From e9803f9e6eaf2bd5d0d4c74126fc9c64e7f9453a Mon Sep 17 00:00:00 2001 From: Allison Date: Thu, 1 Aug 2024 14:47:54 -0700 Subject: [PATCH 01/35] made class --- .../MocoGoal/MocoParameterExpressionGoal.cpp | 36 ++++++++++++ .../MocoGoal/MocoParameterExpressionGoal.h | 58 +++++++++++++++++++ 2 files changed, 94 insertions(+) create mode 100644 OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp create mode 100644 OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.h diff --git a/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp b/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp new file mode 100644 index 0000000000..9102b181ad --- /dev/null +++ b/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp @@ -0,0 +1,36 @@ +// +// Created by Allison John on 8/1/24. +// + +#include "MocoParameterExpressionGoal.h" +#include +#include + +using namespace OpenSim; + +void MocoParameterExpressionGoal::constructProperties() { + constructProperty_expression(""); + constructProperty_parameters(); +} + +void MocoParameterExpressionGoal::initializeOnModelImpl(const Model&) const { + _parameterProg = Lepton::Parser::parse(get_expression()).optimize().createProgram(); +} + +void MocoParameterExpressionGoal::calcIntegrandImpl( + const IntegrandInput& input, double& integrand) const { + + std::map parameterVars; + for (int i=0; i + +namespace OpenSim { + +class OSIMMOCO_API MocoParameterExpressionGoal : public MocoGoal { + OpenSim_DECLARE_ABSTRACT_OBJECT(MocoParameterExpressionGoal, MocoGoal); + +public: + MocoParameterExpressionGoal() { constructProperties(); } + MocoParameterExpressionGoal(std::string name) : MocoGoal(std::move(name)) { + constructProperties(); + } + MocoParameterExpressionGoal(std::string name, double weight) + : MocoGoal(std::move(name), weight) { + constructProperties(); + } + + void setExpression(std::string& expression) { + set_expression(expression); + } + + void addParameter(MocoParameter& parameter, std::string variableName) { + updProperty_parameters().appendValue(parameter); + updProperty_variable_names().appendValue(variableName); + } + +protected: + void initializeOnModelImpl(const Model&) const override; + void calcIntegrandImpl( + const IntegrandInput& input, SimTK::Real& integrand) const override; + void calcGoalImpl( + const GoalInput& input, SimTK::Vector& cost) const override; + //void printDescriptionImpl() const override; + +private: + void constructProperties(); + OpenSim_DECLARE_PROPERTY(expression, std::string, + "The expression string with variables q0-q9."); + // consider a mapping from variable names to parameters instead + OpenSim_DECLARE_LIST_PROPERTY(parameters, MocoParameter, + "MocoParameters to use in the expression."); + OpenSim_DECLARE_LIST_PROPERTY(variable_names, std::string, + "Variable names of the MocoParameters to use in the expression."); + + mutable Lepton::ExpressionProgram _parameterProg; +}; + +} // namespace OpenSim + +#endif // OPENSIM_MOCOPARAMETEREXPRESSIONGOAL_H From 84ccce7d42308d299d0834d9b25f180e21dcd21e Mon Sep 17 00:00:00 2001 From: Allison Date: Thu, 8 Aug 2024 15:40:59 -0700 Subject: [PATCH 02/35] WIP adding getvalue to parameter --- OpenSim/Moco/CMakeLists.txt | 3 ++- OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp | 6 +++++- OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.h | 5 ++++- OpenSim/Moco/MocoParameter.cpp | 4 ++++ OpenSim/Moco/MocoParameter.h | 4 ++++ OpenSim/Moco/Test/testMocoGoals.cpp | 5 +++++ 6 files changed, 24 insertions(+), 3 deletions(-) diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 77219096fd..60d1fc80a5 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -113,7 +113,8 @@ set(MOCO_SOURCES MocoOutputBoundConstraint.h MocoStateBoundConstraint.cpp MocoStateBoundConstraint.h - ) + MocoGoal/MocoParameterExpressionGoal.cpp +) if(OPENSIM_WITH_CASADI) list(APPEND MOCO_SOURCES MocoCasADiSolver/CasOCProblem.h diff --git a/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp b/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp index 9102b181ad..e828ad663b 100644 --- a/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp @@ -19,6 +19,10 @@ void MocoParameterExpressionGoal::initializeOnModelImpl(const Model&) const { void MocoParameterExpressionGoal::calcIntegrandImpl( const IntegrandInput& input, double& integrand) const { + integrand = calcOutputValue(input.state); +} + +double MocoParameterExpressionGoal::calcOutputValue(const SimTK::State& state) const { std::map parameterVars; for (int i=0; i Date: Fri, 9 Aug 2024 16:57:53 -0700 Subject: [PATCH 03/35] draft, errors on test case --- OpenSim/Moco/CMakeLists.txt | 2 +- .../MocoGoal/MocoParameterExpressionGoal.cpp | 35 +++++++++++++------ .../MocoGoal/MocoParameterExpressionGoal.h | 29 ++++++++++----- OpenSim/Moco/MocoParameter.cpp | 21 +++++++++-- OpenSim/Moco/MocoParameter.h | 5 +-- OpenSim/Moco/RegisterTypes_osimMoco.cpp | 2 ++ OpenSim/Moco/Test/testMocoGoals.cpp | 35 ++++++++++++++++--- 7 files changed, 99 insertions(+), 30 deletions(-) diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 60d1fc80a5..5c91bcd28d 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -58,6 +58,7 @@ set(MOCO_SOURCES MocoGoal/MocoInitialVelocityEquilibriumDGFGoal.cpp MocoGoal/MocoInitialForceEquilibriumDGFGoal.h MocoGoal/MocoInitialForceEquilibriumDGFGoal.cpp + MocoGoal/MocoParameterExpressionGoal.cpp MocoGoal/MocoPeriodicityGoal.h MocoGoal/MocoPeriodicityGoal.cpp MocoGoal/MocoStepTimeAsymmetryGoal.h @@ -113,7 +114,6 @@ set(MOCO_SOURCES MocoOutputBoundConstraint.h MocoStateBoundConstraint.cpp MocoStateBoundConstraint.h - MocoGoal/MocoParameterExpressionGoal.cpp ) if(OPENSIM_WITH_CASADI) list(APPEND MOCO_SOURCES diff --git a/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp b/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp index e828ad663b..0128a67e82 100644 --- a/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp @@ -1,6 +1,20 @@ -// -// Created by Allison John on 8/1/24. -// +/* -------------------------------------------------------------------------- * +* OpenSim: MocoParameterExpressionGoal.h * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ #include "MocoParameterExpressionGoal.h" #include @@ -11,30 +25,29 @@ using namespace OpenSim; void MocoParameterExpressionGoal::constructProperties() { constructProperty_expression(""); constructProperty_parameters(); + constructProperty_variable_names(); } void MocoParameterExpressionGoal::initializeOnModelImpl(const Model&) const { - _parameterProg = Lepton::Parser::parse(get_expression()).optimize().createProgram(); + m_parameterProg = Lepton::Parser::parse(get_expression()).optimize().createProgram(); + setRequirements(1, 1); } void MocoParameterExpressionGoal::calcIntegrandImpl( const IntegrandInput& input, double& integrand) const { - integrand = calcOutputValue(input.state); -} - -double MocoParameterExpressionGoal::calcOutputValue(const SimTK::State& state) const { std::map parameterVars; for (int i=0; i namespace OpenSim { @@ -23,7 +37,7 @@ class OSIMMOCO_API MocoParameterExpressionGoal : public MocoGoal { constructProperties(); } - void setExpression(std::string& expression) { + void setExpression(std::string expression) { set_expression(expression); } @@ -42,9 +56,6 @@ class OSIMMOCO_API MocoParameterExpressionGoal : public MocoGoal { private: void constructProperties(); - /** Calculate the Output value for the provided SimTK::State. Do not - call this function until 'initializeOnModelBase()' has been called. */ - double calcOutputValue(const SimTK::State&) const; OpenSim_DECLARE_PROPERTY(expression, std::string, "The expression string with variables q0-q9."); // consider a mapping from variable names to parameters instead @@ -53,7 +64,7 @@ class OSIMMOCO_API MocoParameterExpressionGoal : public MocoGoal { OpenSim_DECLARE_LIST_PROPERTY(variable_names, std::string, "Variable names of the MocoParameters to use in the expression."); - mutable Lepton::ExpressionProgram _parameterProg; + mutable Lepton::ExpressionProgram m_parameterProg; }; } // namespace OpenSim diff --git a/OpenSim/Moco/MocoParameter.cpp b/OpenSim/Moco/MocoParameter.cpp index 9652742a8f..45c35e216c 100644 --- a/OpenSim/Moco/MocoParameter.cpp +++ b/OpenSim/Moco/MocoParameter.cpp @@ -73,8 +73,25 @@ void MocoParameter::constructProperties() { constructProperty_property_element(); } -double getPropertyValue(const SimTK::State& state) const { - state.get +double MocoParameter::getPropertyValue() const{ + OPENSIM_THROW_IF_FRMOBJ(m_property_refs.size() < 1, Exception, + "No properties to get value of.") + const auto& propRef = m_property_refs[0]; + + if (m_data_type == Type_double) { + return static_cast*>(propRef.get())->getValue(); + } + + int elt = get_property_element(); + if (m_data_type == Type_Vec3) { + return static_cast*>(propRef.get())->getValue()[elt]; + } + + if (m_data_type == Type_Vec6) { + return static_cast*>(propRef.get())->getValue()[elt]; + } + + OPENSIM_THROW_FRMOBJ(Exception, "Properties not of a recognized type."); } void MocoParameter::initializeOnModel(Model& model) const { diff --git a/OpenSim/Moco/MocoParameter.h b/OpenSim/Moco/MocoParameter.h index c477818d77..f9c5d5df14 100644 --- a/OpenSim/Moco/MocoParameter.h +++ b/OpenSim/Moco/MocoParameter.h @@ -20,9 +20,10 @@ #include "MocoBounds.h" -#include #include +#include #include +#include namespace OpenSim { @@ -144,7 +145,7 @@ class OSIMMOCO_API MocoParameter : public Object { /** Get the value of the property at the given state. * @throws if the value is not the same at all paths. */ - double getPropertyValue(const SimTK::State& state) const; + double getPropertyValue() const; /** For use by solvers. This performs error checks and caches information about the model that is useful during the optimization. diff --git a/OpenSim/Moco/RegisterTypes_osimMoco.cpp b/OpenSim/Moco/RegisterTypes_osimMoco.cpp index 581d87b663..41c6588a04 100644 --- a/OpenSim/Moco/RegisterTypes_osimMoco.cpp +++ b/OpenSim/Moco/RegisterTypes_osimMoco.cpp @@ -42,6 +42,7 @@ #include "MocoGoal/MocoMarkerTrackingGoal.h" #include "MocoGoal/MocoOrientationTrackingGoal.h" #include "MocoGoal/MocoOutputGoal.h" +#include "MocoGoal/MocoParameterExpressionGoal.h" #include "MocoGoal/MocoPeriodicityGoal.h" #include "MocoGoal/MocoStateTrackingGoal.h" #include "MocoGoal/MocoStepLengthAsymmetryGoal.h" @@ -89,6 +90,7 @@ OSIMMOCO_API void RegisterTypes_osimMoco() { Object::registerType(MocoTranslationTrackingGoal()); Object::registerType(MocoAngularVelocityTrackingGoal()); Object::registerType(MocoAccelerationTrackingGoal()); + Object::registerType(MocoParameterExpressionGoal()); Object::registerType(MocoPeriodicityGoalPair()); Object::registerType(MocoPeriodicityGoal()); Object::registerType(MocoOutputGoal()); diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index f0155cdbdd..0932928789 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -16,17 +16,18 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ +#include "Testing.h" +#include + #include #include #include -#include +#include #include +#include #include #include -#include -#include "Testing.h" - using Catch::Approx; using Catch::Matchers::ContainsSubstring; @@ -1414,7 +1415,31 @@ TEST_CASE("MocoFrameDistanceConstraint de/serialization") { } } -TEST_CASE("MocoParameterExpressionGoal", "", MocoCasADiSolver, +TEMPLATE_TEST_CASE("MocoParameterExpressionGoal", "", MocoCasADiSolver, MocoTropterSolver) { + MocoStudy study; + study.setName("oscillator_mass"); + Model model = ModelFactory::createSlidingPointMass(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(model); + mp.setTimeBounds(0, 3); + mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", MocoBounds(0, 10)); + + auto* goal = mp.addGoal(); + goal->setExpression("50-10*q"); + goal->addParameter(*parameter, "q"); + parameter->initializeOnModel(model); // should not have to do this here + + + auto& ms = study.initSolver(); + ms.set_num_mesh_intervals(25); + + MocoSolution sol = study.solve(); + //sol.write("testMocoParameters_testOscillatorMass_sol.sto"); + CHECK(sol.getParameter("oscillator_mass") == + Catch::Approx(5).epsilon(0.003)); } From d7194316e7572b56e2265db207480e4418fd716b Mon Sep 17 00:00:00 2001 From: Allison Date: Tue, 13 Aug 2024 17:03:11 -0700 Subject: [PATCH 04/35] goal stores properties itself --- OpenSim/Moco/CMakeLists.txt | 3 +- .../MocoExpressionBasedParameterGoal.cpp | 108 ++++++++++++++++++ ...l.h => MocoExpressionBasedParameterGoal.h} | 32 ++++-- .../MocoGoal/MocoParameterExpressionGoal.cpp | 53 --------- OpenSim/Moco/MocoParameter.h | 4 + OpenSim/Moco/RegisterTypes_osimMoco.cpp | 4 +- OpenSim/Moco/Test/testMocoGoals.cpp | 12 +- OpenSim/Moco/osimMoco.h | 1 + 8 files changed, 147 insertions(+), 70 deletions(-) create mode 100644 OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp rename OpenSim/Moco/MocoGoal/{MocoParameterExpressionGoal.h => MocoExpressionBasedParameterGoal.h} (73%) delete mode 100644 OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 5c91bcd28d..3ef720001b 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -40,6 +40,8 @@ set(MOCO_SOURCES MocoGoal/MocoControlGoal.cpp MocoGoal/MocoControlTrackingGoal.h MocoGoal/MocoControlTrackingGoal.cpp + MocoGoal/MocoExpressionBasedParameterGoal.h + MocoGoal/MocoExpressionBasedParameterGoal.cpp MocoGoal/MocoJointReactionGoal.h MocoGoal/MocoJointReactionGoal.cpp MocoGoal/MocoOrientationTrackingGoal.h @@ -58,7 +60,6 @@ set(MOCO_SOURCES MocoGoal/MocoInitialVelocityEquilibriumDGFGoal.cpp MocoGoal/MocoInitialForceEquilibriumDGFGoal.h MocoGoal/MocoInitialForceEquilibriumDGFGoal.cpp - MocoGoal/MocoParameterExpressionGoal.cpp MocoGoal/MocoPeriodicityGoal.h MocoGoal/MocoPeriodicityGoal.cpp MocoGoal/MocoStepTimeAsymmetryGoal.h diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp new file mode 100644 index 0000000000..882f3add65 --- /dev/null +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -0,0 +1,108 @@ +/* -------------------------------------------------------------------------- * +* OpenSim: MocoParameterExpressionGoal.h * + * -------------------------------------------------------------------------- * + * Copyright (c) 2024 Stanford University and the Authors * + * * + * Author(s): Allison John * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "MocoExpressionBasedParameterGoal.h" +#include +#include + +#include + +using namespace OpenSim; + +void MocoExpressionBasedParameterGoal::constructProperties() { + constructProperty_expression(""); + constructProperty_parameters(); + constructProperty_variable_names(); +} + +void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) const { + m_parameterProg = Lepton::Parser::parse(get_expression()).optimize().createProgram(); + setRequirements(1, 1); + + // store parameter property ref + for (int i = 0; i < getProperty_parameters().size(); i++) { + std::string componentPath = get_parameters(i).getComponentPaths()[0]; // first one bc they should all be the same + const auto& component = model.getComponent(componentPath); + // Get component property. + const auto* ap = &component.getPropertyByName(get_parameters(i).getPropertyName()); + m_property_refs.emplace_back(ap); + + // get the type of the property, and element + OPENSIM_THROW_IF_FRMOBJ(ap->isListProperty(), Exception, + "MocoParameter does not support list properties."); + + // Type detection and property element value error checking. + if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_double); + } else { + if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_Vec3); + m_indices.emplace_back(get_parameters(i).getElement()); + } + else if (dynamic_cast*>(ap)) { + m_data_types.emplace_back(Type_Vec6); + m_indices.emplace_back(get_parameters(i).getElement()); + } + else { + OPENSIM_THROW_FRMOBJ(Exception, + "Data type of specified model property not supported."); + } + } + + m_property_refs.emplace_back(ap); + } +} + +double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { + OPENSIM_THROW_IF_FRMOBJ(m_property_refs.size() < 1, Exception, + "No properties to get value of.") + const auto& propRef = m_property_refs[i]; + + if (m_data_types[i] == Type_double) { + return static_cast*>(propRef.get())->getValue(); + } + + int elt = m_indices[i]; + if (m_data_types[i] == Type_Vec3) { + return static_cast*>(propRef.get())->getValue()[elt]; + } + + if (m_data_types[i] == Type_Vec6) { + return static_cast*>(propRef.get())->getValue()[elt]; + } + + OPENSIM_THROW_FRMOBJ(Exception, "Properties not of a recognized type."); +} + +void MocoExpressionBasedParameterGoal::calcIntegrandImpl( + const IntegrandInput& input, double& integrand) const { + + std::map parameterVars; + for (int i=0; i +#include +#include +#include +#include + namespace OpenSim { +class Model; -class OSIMMOCO_API MocoParameterExpressionGoal : public MocoGoal { - OpenSim_DECLARE_CONCRETE_OBJECT(MocoParameterExpressionGoal, MocoGoal); +class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { + OpenSim_DECLARE_CONCRETE_OBJECT(MocoExpressionBasedParameterGoal, MocoGoal); public: - MocoParameterExpressionGoal() { constructProperties(); } - MocoParameterExpressionGoal(std::string name) : MocoGoal(std::move(name)) { + MocoExpressionBasedParameterGoal() { constructProperties(); } + MocoExpressionBasedParameterGoal(std::string name) : MocoGoal(std::move(name)) { constructProperties(); } - MocoParameterExpressionGoal(std::string name, double weight) + MocoExpressionBasedParameterGoal(std::string name, double weight) : MocoGoal(std::move(name), weight) { constructProperties(); } @@ -47,7 +53,7 @@ class OSIMMOCO_API MocoParameterExpressionGoal : public MocoGoal { } protected: - void initializeOnModelImpl(const Model&) const override; + void initializeOnModelImpl(const Model& model) const override; void calcIntegrandImpl( const IntegrandInput& input, SimTK::Real& integrand) const override; void calcGoalImpl( @@ -65,6 +71,16 @@ class OSIMMOCO_API MocoParameterExpressionGoal : public MocoGoal { "Variable names of the MocoParameters to use in the expression."); mutable Lepton::ExpressionProgram m_parameterProg; + mutable std::vector> m_property_refs; + enum DataType { + Type_double, + Type_Vec3, + Type_Vec6 + }; + mutable std::vector m_data_types; + mutable std::vector m_indices; + + double getPropertyValue(int i) const; }; } // namespace OpenSim diff --git a/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp b/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp deleted file mode 100644 index 0128a67e82..0000000000 --- a/OpenSim/Moco/MocoGoal/MocoParameterExpressionGoal.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/* -------------------------------------------------------------------------- * -* OpenSim: MocoParameterExpressionGoal.h * - * -------------------------------------------------------------------------- * - * Copyright (c) 2024 Stanford University and the Authors * - * * - * Author(s): Allison John * - * * - * Licensed under the Apache License, Version 2.0 (the "License"); you may * - * not use this file except in compliance with the License. You may obtain a * - * copy of the License at http://www.apache.org/licenses/LICENSE-2.0 * - * * - * Unless required by applicable law or agreed to in writing, software * - * distributed under the License is distributed on an "AS IS" BASIS, * - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * - * See the License for the specific language governing permissions and * - * limitations under the License. * - * -------------------------------------------------------------------------- */ - -#include "MocoParameterExpressionGoal.h" -#include -#include - -using namespace OpenSim; - -void MocoParameterExpressionGoal::constructProperties() { - constructProperty_expression(""); - constructProperty_parameters(); - constructProperty_variable_names(); -} - -void MocoParameterExpressionGoal::initializeOnModelImpl(const Model&) const { - m_parameterProg = Lepton::Parser::parse(get_expression()).optimize().createProgram(); - setRequirements(1, 1); -} - -void MocoParameterExpressionGoal::calcIntegrandImpl( - const IntegrandInput& input, double& integrand) const { - - std::map parameterVars; - for (int i=0; i #include #include -#include +#include #include #include #include @@ -1415,7 +1415,7 @@ TEST_CASE("MocoFrameDistanceConstraint de/serialization") { } } -TEMPLATE_TEST_CASE("MocoParameterExpressionGoal", "", MocoCasADiSolver, +TEMPLATE_TEST_CASE("MocoExpressionBasedParameterGoal", "", MocoCasADiSolver, MocoTropterSolver) { MocoStudy study; study.setName("oscillator_mass"); @@ -1428,10 +1428,10 @@ TEMPLATE_TEST_CASE("MocoParameterExpressionGoal", "", MocoCasADiSolver, auto* parameter = mp.addParameter("sphere_mass", "body", "mass", MocoBounds(0, 10)); - auto* goal = mp.addGoal(); - goal->setExpression("50-10*q"); + auto* goal = mp.addGoal(); + goal->setExpression("(q-5)^2"); goal->addParameter(*parameter, "q"); - parameter->initializeOnModel(model); // should not have to do this here + //parameter->initializeOnModel(model); // should not have to do this here auto& ms = study.initSolver(); @@ -1440,6 +1440,6 @@ TEMPLATE_TEST_CASE("MocoParameterExpressionGoal", "", MocoCasADiSolver, MocoSolution sol = study.solve(); //sol.write("testMocoParameters_testOscillatorMass_sol.sto"); - CHECK(sol.getParameter("oscillator_mass") == + CHECK(sol.getParameter("sphere_mass") == Catch::Approx(5).epsilon(0.003)); } diff --git a/OpenSim/Moco/osimMoco.h b/OpenSim/Moco/osimMoco.h index c94688ec6d..cc39e2c268 100644 --- a/OpenSim/Moco/osimMoco.h +++ b/OpenSim/Moco/osimMoco.h @@ -35,6 +35,7 @@ #include "MocoGoal/MocoContactTrackingGoal.h" #include "MocoGoal/MocoControlGoal.h" #include "MocoGoal/MocoControlTrackingGoal.h" +#include "MocoGoal/MocoExpressionBasedParameterGoal.h" #include "MocoGoal/MocoInitialActivationGoal.h" #include "MocoGoal/MocoInitialForceEquilibriumDGFGoal.h" #include "MocoGoal/MocoInitialVelocityEquilibriumDGFGoal.h" From a92bc2bd63cc2c4cf4e1b1a0f08d3dbb4d98331e Mon Sep 17 00:00:00 2001 From: Allison Date: Tue, 13 Aug 2024 17:06:41 -0700 Subject: [PATCH 05/35] name change in comment --- OpenSim/Moco/CMakeLists.txt | 2 +- OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp | 2 +- OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 3ef720001b..30b14913c1 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -115,7 +115,7 @@ set(MOCO_SOURCES MocoOutputBoundConstraint.h MocoStateBoundConstraint.cpp MocoStateBoundConstraint.h -) + ) if(OPENSIM_WITH_CASADI) list(APPEND MOCO_SOURCES MocoCasADiSolver/CasOCProblem.h diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index 882f3add65..036a5f7627 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -1,5 +1,5 @@ /* -------------------------------------------------------------------------- * -* OpenSim: MocoParameterExpressionGoal.h * +* OpenSim: MocoExpressionBasedParameterGoal.h * * -------------------------------------------------------------------------- * * Copyright (c) 2024 Stanford University and the Authors * * * diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index e0d41c9ad8..461d86cbdd 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -1,7 +1,7 @@ #ifndef OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H #define OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H /* -------------------------------------------------------------------------- * - * OpenSim: MocoParameterExpressionGoal.h * + * OpenSim: MocoExpressionBasedParameterGoal.h * * -------------------------------------------------------------------------- * * Copyright (c) 2024 Stanford University and the Authors * * * From 1406b4dc15491da3ea868cf8dd54bb9be52deef5 Mon Sep 17 00:00:00 2001 From: Allison Date: Wed, 14 Aug 2024 12:23:19 -0700 Subject: [PATCH 06/35] two parameter test --- .../MocoExpressionBasedParameterGoal.cpp | 12 ++-- OpenSim/Moco/MocoParameter.cpp | 21 ------ OpenSim/Moco/MocoParameter.h | 4 -- OpenSim/Moco/Test/testMocoGoals.cpp | 70 +++++++++++++------ 4 files changed, 55 insertions(+), 52 deletions(-) diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index 036a5f7627..5806803622 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -63,14 +63,13 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) "Data type of specified model property not supported."); } } - - m_property_refs.emplace_back(ap); } + // warning if not all parameters are in the expression } double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { - OPENSIM_THROW_IF_FRMOBJ(m_property_refs.size() < 1, Exception, - "No properties to get value of.") + OPENSIM_THROW_IF_FRMOBJ(m_property_refs.size() < i+1, Exception, + "Doesn't have that many parameters.") const auto& propRef = m_property_refs[i]; if (m_data_types[i] == Type_double) { @@ -93,9 +92,8 @@ void MocoExpressionBasedParameterGoal::calcIntegrandImpl( const IntegrandInput& input, double& integrand) const { std::map parameterVars; - for (int i=0; i*>(propRef.get())->getValue(); - } - - int elt = get_property_element(); - if (m_data_type == Type_Vec3) { - return static_cast*>(propRef.get())->getValue()[elt]; - } - - if (m_data_type == Type_Vec6) { - return static_cast*>(propRef.get())->getValue()[elt]; - } - - OPENSIM_THROW_FRMOBJ(Exception, "Properties not of a recognized type."); -} - void MocoParameter::initializeOnModel(Model& model) const { OPENSIM_THROW_IF_FRMOBJ(getProperty_component_paths().empty(), Exception, diff --git a/OpenSim/Moco/MocoParameter.h b/OpenSim/Moco/MocoParameter.h index 41917cb655..26d6800fd6 100644 --- a/OpenSim/Moco/MocoParameter.h +++ b/OpenSim/Moco/MocoParameter.h @@ -143,10 +143,6 @@ class OSIMMOCO_API MocoParameter : public Object { void appendComponentPath(const std::string& componentPath) { append_component_paths(componentPath); } - /** Get the value of the property at the given state. - * @throws if the value is not the same at all paths. */ - double getPropertyValue() const; - int getElement() const { return get_property_element(); } diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index cc29b7e01e..5efab01e00 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -1417,29 +1417,59 @@ TEST_CASE("MocoFrameDistanceConstraint de/serialization") { TEMPLATE_TEST_CASE("MocoExpressionBasedParameterGoal", "", MocoCasADiSolver, MocoTropterSolver) { - MocoStudy study; - study.setName("oscillator_mass"); - Model model = ModelFactory::createSlidingPointMass(); - MocoProblem& mp = study.updProblem(); - mp.setModelAsCopy(model); - mp.setTimeBounds(0, 3); - mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); - mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); - - auto* parameter = mp.addParameter("sphere_mass", "body", "mass", MocoBounds(0, 10)); + /*SECTION("Linear sphere, mass goal") { + // takes 20 seconds :/ + MocoStudy study; + Model model = ModelFactory::createSlidingPointMass(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(model); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", MocoBounds(0, 10)); + auto* mass_goal = mp.addGoal(); + mass_goal->setExpression("(q-4)^2"); + mass_goal->addParameter(*parameter, "q"); + auto* effort_goal = mp.addGoal(); + effort_goal->setWeight(0.001); + effort_goal->setName("effort"); - auto* goal = mp.addGoal(); - goal->setExpression("(q-5)^2"); - goal->addParameter(*parameter, "q"); - //parameter->initializeOnModel(model); // should not have to do this here + auto& ms = study.initSolver(); + ms.set_num_mesh_intervals(25); + MocoSolution sol = study.solve(); + // 3.998 + CHECK(sol.getParameter("sphere_mass") == Catch::Approx(4).epsilon(1e-2)); + }*/ - auto& ms = study.initSolver(); - ms.set_num_mesh_intervals(25); + SECTION("Double mass goal") { + // takes 22 seconds :/ + MocoStudy study; + auto model = createDoubleSlidingMassModel(); + model->initSystem(); + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(*model); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}); + mp.setStateInfo("/slider/position/speed", {-20, 20}); + mp.setStateInfo("/slider2/position/value", {-5, 5}, 1, {1.2, 1.3}); + mp.setStateInfo("/slider2/position/speed", {-20, 20}); + + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", MocoBounds(0, 10)); + auto* parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", MocoBounds(0, 10)); + int total_weight = 7; + auto* mass_goal = mp.addGoal(); + mass_goal->setExpression(fmt::format("(p+q-{})^2", total_weight)); + mass_goal->addParameter(*parameter, "p"); + mass_goal->addParameter(*parameter2, "q"); - MocoSolution sol = study.solve(); - //sol.write("testMocoParameters_testOscillatorMass_sol.sto"); + auto& ms = study.initSolver(); + ms.set_num_mesh_intervals(25); + MocoSolution sol = study.solve(); - CHECK(sol.getParameter("sphere_mass") == - Catch::Approx(5).epsilon(0.003)); + // 3.7 and 3.3 + CHECK(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass") == + Catch::Approx(total_weight).epsilon(1e-9)); + } } From 0a2d634649b2950aaa9df68540f69887582bd6ef Mon Sep 17 00:00:00 2001 From: Allison Date: Wed, 14 Aug 2024 14:32:37 -0700 Subject: [PATCH 07/35] separate tropter and casadi tests --- .../MocoExpressionBasedParameterGoal.cpp | 2 +- .../MocoExpressionBasedParameterGoal.h | 5 + OpenSim/Moco/MocoParameter.h | 1 - OpenSim/Moco/Test/testMocoGoals.cpp | 108 ++++++++++++++++-- 4 files changed, 106 insertions(+), 10 deletions(-) diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index 5806803622..ed8854124f 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -68,7 +68,7 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) } double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { - OPENSIM_THROW_IF_FRMOBJ(m_property_refs.size() < i+1, Exception, + OPENSIM_THROW_IF_FRMOBJ((int) m_property_refs.size() < i + 1, Exception, "Doesn't have that many parameters.") const auto& propRef = m_property_refs[i]; diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index 461d86cbdd..48ea072efe 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -30,6 +30,11 @@ namespace OpenSim { class Model; +/** Minimize an arithmetic expression of parameters. This goal supports any +number of MocoParameters that are combined into a single goal. The expression +string should match the Lepton (lightweight expression parser) format. + +@ingroup mocogoal */ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoExpressionBasedParameterGoal, MocoGoal); diff --git a/OpenSim/Moco/MocoParameter.h b/OpenSim/Moco/MocoParameter.h index 26d6800fd6..d41abeda38 100644 --- a/OpenSim/Moco/MocoParameter.h +++ b/OpenSim/Moco/MocoParameter.h @@ -142,7 +142,6 @@ class OSIMMOCO_API MocoParameter : public Object { { set_property_name(propertyName); } void appendComponentPath(const std::string& componentPath) { append_component_paths(componentPath); } - int getElement() const { return get_property_element(); } diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index 5efab01e00..628287ad6a 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -1415,10 +1416,9 @@ TEST_CASE("MocoFrameDistanceConstraint de/serialization") { } } -TEMPLATE_TEST_CASE("MocoExpressionBasedParameterGoal", "", MocoCasADiSolver, - MocoTropterSolver) { - /*SECTION("Linear sphere, mass goal") { - // takes 20 seconds :/ +TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { + SECTION("mass goal") { + // takes 20 seconds MocoStudy study; Model model = ModelFactory::createSlidingPointMass(); MocoProblem& mp = study.updProblem(); @@ -1435,16 +1435,16 @@ TEMPLATE_TEST_CASE("MocoExpressionBasedParameterGoal", "", MocoCasADiSolver, effort_goal->setWeight(0.001); effort_goal->setName("effort"); - auto& ms = study.initSolver(); + auto& ms = study.initTropterSolver(); ms.set_num_mesh_intervals(25); MocoSolution sol = study.solve(); // 3.998 CHECK(sol.getParameter("sphere_mass") == Catch::Approx(4).epsilon(1e-2)); - }*/ + } SECTION("Double mass goal") { - // takes 22 seconds :/ + // takes 22 seconds MocoStudy study; auto model = createDoubleSlidingMassModel(); model->initSystem(); @@ -1464,7 +1464,7 @@ TEMPLATE_TEST_CASE("MocoExpressionBasedParameterGoal", "", MocoCasADiSolver, mass_goal->addParameter(*parameter, "p"); mass_goal->addParameter(*parameter2, "q"); - auto& ms = study.initSolver(); + auto& ms = study.initTropterSolver(); ms.set_num_mesh_intervals(25); MocoSolution sol = study.solve(); @@ -1473,3 +1473,95 @@ TEMPLATE_TEST_CASE("MocoExpressionBasedParameterGoal", "", MocoCasADiSolver, Catch::Approx(total_weight).epsilon(1e-9)); } } + +// from testMocoParameters +const double STIFFNESS = 100.0; // N/m +const double MASS = 5.0; // kg +const double FINAL_TIME = SimTK::Pi * sqrt(MASS / STIFFNESS); +std::unique_ptr createOscillatorTwoSpringsModel() { + auto model = make_unique(); + model->setName("oscillator_two_springs"); + model->set_gravity(SimTK::Vec3(0, 0, 0)); + auto* body = new Body("body", MASS, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + + // Allows translation along x. + auto* joint = new SliderJoint("slider", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* spring1 = new SpringGeneralizedForce(); + spring1->setName("spring1"); + spring1->set_coordinate("position"); + spring1->setRestLength(0.0); + spring1->setStiffness(0.25*STIFFNESS); + spring1->setViscosity(0.0); + model->addComponent(spring1); + + auto* spring2 = new SpringGeneralizedForce(); + spring2->setName("spring2"); + spring2->set_coordinate("position"); + spring2->setRestLength(0.0); + spring2->setStiffness(0.25*STIFFNESS); + spring2->setViscosity(0.0); + model->addComponent(spring2); + + return model; +} + +TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { + int N = 25; + + MocoStudy study; + study.setName("oscillator_spring_stiffnesses"); + MocoProblem& mp = study.updProblem(); + mp.setModel(createOscillatorTwoSpringsModel()); + mp.setTimeBounds(0, FINAL_TIME); + mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + SECTION("single parameter") { + // Optimize a single stiffness value and apply to both springs. + std::vector components = {"spring1", "spring2"}; + auto* parameter = mp.addParameter("spring_stiffness", components, + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal(); + spring_goal->setExpression(fmt::format("(p-{})^2", 0.5*STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(N); + ms.set_parameters_require_initsystem(false); // faster, still works with spring stiffness + + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") == + Catch::Approx(0.5*STIFFNESS).epsilon(1e-6)); + } + + SECTION("two parameters") { + // Optimize a single stiffness value and apply to both springs. + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal(); + spring_goal->setExpression(fmt::format("(p+q-{})^2", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + spring_goal->addParameter(*parameter2, "q"); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(N); + ms.set_parameters_require_initsystem(false); // faster, still works with spring stiffness + + MocoSolution sol = study.solve(); + + log_cout("{} {}", sol.getParameter("spring_stiffness") , sol.getParameter("spring2_stiffness")); + + CHECK(sol.getParameter("spring_stiffness") + sol.getParameter("spring2_stiffness") == + Catch::Approx(STIFFNESS).epsilon(1e-6)); + } +} From c60b0e5850ca09655bb2e884e68980a5d1f69514 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 14 Aug 2024 16:46:10 -0700 Subject: [PATCH 08/35] Add Outputs to `ExpressionBasedCoordinateForce` and `ExpressionBasedPointToPointForce` --- .../Model/ExpressionBasedCoordinateForce.cpp | 21 +++++++-------- .../Model/ExpressionBasedCoordinateForce.h | 27 +++++++++++-------- .../ExpressionBasedPointToPointForce.cpp | 3 +-- .../Model/ExpressionBasedPointToPointForce.h | 7 ++++- OpenSim/Simulation/Test/testForces.cpp | 22 ++++++++++++--- 5 files changed, 51 insertions(+), 29 deletions(-) diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp index 637622f9f2..f05bf5403d 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp @@ -53,8 +53,8 @@ ExpressionBasedCoordinateForce::ExpressionBasedCoordinateForce( setExpression(expression); } -// Set the expression for the force function and create it's lepton program -void ExpressionBasedCoordinateForce::setExpression(const string& expression) +// Set the expression for the force function and create it's lepton program +void ExpressionBasedCoordinateForce::setExpression(const string& expression) { set_expression(expression); } @@ -68,7 +68,7 @@ void ExpressionBasedCoordinateForce::setExpression(const string& expression) */ void ExpressionBasedCoordinateForce::setNull() { - setAuthors("Nabeel Allana"); + setAuthors("Nabeel Allana"); } //_____________________________________________________________________________ @@ -94,9 +94,9 @@ void ExpressionBasedCoordinateForce::extendConnectToModel(Model& aModel) string& expression = upd_expression(); expression.erase( - remove_if(expression.begin(), expression.end(), ::isspace), + remove_if(expression.begin(), expression.end(), ::isspace), expression.end() ); - + _forceProg = Lepton::Parser::parse(expression).optimize().createProgram(); // Look up the coordinate @@ -105,7 +105,7 @@ void ExpressionBasedCoordinateForce::extendConnectToModel(Model& aModel) throw (Exception(errorMessage.c_str())); } _coord = &_model->updCoordinateSet().get(coordName); - + if(getName() == "") setName("expressionCoordForce_"+coordName); } @@ -124,8 +124,8 @@ void ExpressionBasedCoordinateForce:: // Computing //============================================================================= // Compute and apply the force -void ExpressionBasedCoordinateForce::computeForce(const SimTK::State& s, - SimTK::Vector_& bodyForces, +void ExpressionBasedCoordinateForce::computeForce(const SimTK::State& s, + SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const { applyGeneralizedForce(s, *_coord, calcExpressionForce(s), generalizedForces); @@ -147,8 +147,7 @@ double ExpressionBasedCoordinateForce::calcExpressionForce(const SimTK::State& s // get the force magnitude that has already been computed const double& ExpressionBasedCoordinateForce:: - getForceMagnitude(const SimTK::State& s) -{ + getForceMagnitude(const SimTK::State& s) const { return getCacheVariableValue(s, _forceMagnitudeCV); } @@ -156,7 +155,7 @@ const double& ExpressionBasedCoordinateForce:: //============================================================================= // Reporting //============================================================================= -// Provide names of the quantities (column labels) of the force value(s) +// Provide names of the quantities (column labels) of the force value(s) // reported. Array ExpressionBasedCoordinateForce::getRecordLabels() const { OpenSim::Array labels(""); diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h index 000cec60ae..694bb32525 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h @@ -42,6 +42,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); "and its time derivative (qdot). Note, expression cannot have any whitespace" "separating characters"); //============================================================================== +// OUTPUTS +//============================================================================== + OpenSim_DECLARE_OUTPUT(force, double, getForceMagnitude, + SimTK::Stage::Dynamics); +//============================================================================== // PUBLIC METHODS //============================================================================== /** Default constructor. **/ @@ -58,7 +63,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); /** * Coordinate */ - void setCoordinateName(const std::string& coord) + void setCoordinateName(const std::string& coord) { set_coordinate(coord); } const std::string& getCoordinateName() const {return get_coordinate();} @@ -66,32 +71,32 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); * %Set the mathematical expression that defines the force magnitude of this * coordinate force in terms of the coordinate value (q) and its * time derivative (qdot). Expressions with C-mathematical operations - * such as +,-,*,/ and common functions: exp, pow, sqrt, sin, cos, tan, + * such as +,-,*,/ and common functions: exp, pow, sqrt, sin, cos, tan, * and so on are acceptable. * NOTE: a limitation is that the expression may not contain whitespace * @param expression string containing the mathematical expression that - * defines the coordinate force + * defines the coordinate force */ void setExpression(const std::string& expression); - /** - * Get the computed Force magnitude determined by evaluating the + /** + * Get the computed Force magnitude determined by evaluating the * expression above. Note, computeForce must be evaluated first, * and this is done automatically if the system is realized to Dynamics * @param state const state (reference) for the model * @return const double ref to the force magnitude */ - const double& getForceMagnitude(const SimTK::State& state); + const double& getForceMagnitude(const SimTK::State& state) const; //============================================================================== // COMPUTATION //============================================================================== - /** Compute the coordinate force based on the user-defined expression + /** Compute the coordinate force based on the user-defined expression and apply it to the model */ - void computeForce(const SimTK::State& state, - SimTK::Vector_& bodyForces, + void computeForce(const SimTK::State& state, + SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const override; //-------------------------------------------------------------------------- @@ -103,7 +108,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); //============================================================================== // Reporting //============================================================================== - /** + /** * Provide name(s) of the quantities (column labels) of the force value(s) to be reported */ OpenSim::Array getRecordLabels() const override; @@ -112,7 +117,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); */ OpenSim::Array getRecordValues(const SimTK::State& state) const override; - + protected: //============================================================================== diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp index a1157bb332..5560d0d942 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.cpp @@ -202,8 +202,7 @@ void ExpressionBasedPointToPointForce::computeForce(const SimTK::State& s, // get the force magnitude that has already been computed const double& ExpressionBasedPointToPointForce:: - getForceMagnitude(const SimTK::State& s) -{ + getForceMagnitude(const SimTK::State& s) const { return getCacheVariableValue(s, _forceMagnitudeCV); } diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h index 311d1fe504..b913561c72 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h @@ -71,6 +71,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); "the distance (d) between the points and its time derivative (ddot). " "Note, expression cannot have any whitespace separating characters."); +//============================================================================== +// OUTPUTS +//============================================================================== + OpenSim_DECLARE_OUTPUT(force, double, getForceMagnitude, + SimTK::Stage::Dynamics); //============================================================================== // PUBLIC METHODS @@ -132,7 +137,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); * @param state const state (reference) for the model * @return const double ref to the force magnitude */ - const double& getForceMagnitude(const SimTK::State& state); + const double& getForceMagnitude(const SimTK::State& state) const; //-------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 158371ca0e..6cef8bb26d 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -33,6 +33,7 @@ // 7. RotationalCoordinateLimitForce // 8. ExternalForce // 9. PathSpring +// 10. ExpressionBasedCoordinateForce // 10. ExpressionBasedPointToPointForce // 11. Blankevoort1991Ligament // @@ -139,9 +140,10 @@ TEST_CASE("testExpressionBasedCoordinateForce") { osimModel.setGravity(gravity_vec); // ode for basic mass-spring-dampener system - ExpressionBasedCoordinateForce spring("ball_h", "-10*q-5*qdot"); + ExpressionBasedCoordinateForce* spring = + new ExpressionBasedCoordinateForce("ball_h", "-10*q-5*qdot"); - osimModel.addForce(&spring); + osimModel.addForce(spring); // Create the force reporter ForceReporter* reporter = new ForceReporter(&osimModel); @@ -182,12 +184,22 @@ TEST_CASE("testExpressionBasedCoordinateForce") { dh; ASSERT_EQUAL(height, pos(1), 1e-6); + + double ball_h = sliderCoord.getValue(osim_state); + double ball_h_dot = sliderCoord.getSpeedValue(osim_state); + + double analytical_force = -10 * ball_h - 5 * ball_h_dot; + double model_force = spring->getForceMagnitude(osim_state); + double output_force = + spring->getOutputValue(osim_state, "force"); + ASSERT_EQUAL(analytical_force, model_force, 1e-5); + ASSERT_EQUAL(analytical_force, output_force, 1e-5); } // Test copying - ExpressionBasedCoordinateForce* copyOfSpring = spring.clone(); + ExpressionBasedCoordinateForce* copyOfSpring = spring->clone(); - ASSERT(*copyOfSpring == spring); + ASSERT(*copyOfSpring == *spring); osimModel.print("ExpressionBasedCoordinateForceModel.osim"); @@ -264,6 +276,7 @@ TEST_CASE("testExpressionBasedPointToPointForce") { // Now check that the force reported by spring double model_force = p2pForce->getForceMagnitude(state); + double output_force = p2pForce->getOutputValue(state, "force"); // Save the forces // reporter->getForceStorage().print("path_spring_forces.mot"); @@ -280,6 +293,7 @@ TEST_CASE("testExpressionBasedPointToPointForce") { // something is wrong if the block does not reach equilibrium ASSERT_EQUAL(analytical_force, model_force, 1e-5); + ASSERT_EQUAL(analytical_force, output_force, 1e-5); // Before exiting lets see if copying the P2P force works ExpressionBasedPointToPointForce* copyOfP2pForce = p2pForce->clone(); From 418f4d86feff23626383bbe02a9aaa7cd5357290 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 14 Aug 2024 16:53:06 -0700 Subject: [PATCH 09/35] Update CHANGELOG --- CHANGELOG.md | 1 + OpenSim/Simulation/Test/testForces.cpp | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5e6c1e81e8..0b938cd137 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -75,6 +75,7 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from occurring in the output. (#3867) +- Added `Output`s to `ExpressionBasedCoordinateForce` and `ExpressionBasedPointToPointForce` for accessing force values. (#3872) v4.5 diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 6cef8bb26d..e5c090920f 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -172,6 +172,7 @@ TEST_CASE("testExpressionBasedCoordinateForce") { osimModel.getMultibodySystem().realize(osim_state, Stage::Acceleration); Vec3 pos = ball.findStationLocationInGround(osim_state, Vec3(0)); + // Check ball height against analytical solution. double height = exp(-1 * zeta * omega * osim_state.getTime()) * ((start_h - dh) * @@ -182,13 +183,12 @@ TEST_CASE("testExpressionBasedCoordinateForce") { sin(damp_freq * osim_state.getTime()))) + dh; - ASSERT_EQUAL(height, pos(1), 1e-6); + // Check that the force reported by spring is correct. double ball_h = sliderCoord.getValue(osim_state); double ball_h_dot = sliderCoord.getSpeedValue(osim_state); - - double analytical_force = -10 * ball_h - 5 * ball_h_dot; + double analytical_force = -10*ball_h - 5*ball_h_dot; double model_force = spring->getForceMagnitude(osim_state); double output_force = spring->getOutputValue(osim_state, "force"); From 9ea5a59e0b5ff244ffca7d564b4304cdba1ea274 Mon Sep 17 00:00:00 2001 From: Allison Date: Wed, 14 Aug 2024 16:59:41 -0700 Subject: [PATCH 10/35] error for missing variables, comments --- .../MocoExpressionBasedParameterGoal.cpp | 38 ++++++++- .../MocoExpressionBasedParameterGoal.h | 26 ++++++- OpenSim/Moco/Test/testMocoGoals.cpp | 77 +++++++++++++------ 3 files changed, 110 insertions(+), 31 deletions(-) diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index ed8854124f..10d351254d 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -17,8 +17,10 @@ * -------------------------------------------------------------------------- */ #include "MocoExpressionBasedParameterGoal.h" -#include + +#include #include +#include #include @@ -30,7 +32,8 @@ void MocoExpressionBasedParameterGoal::constructProperties() { constructProperty_variable_names(); } -void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) const { +void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) + const { m_parameterProg = Lepton::Parser::parse(get_expression()).optimize().createProgram(); setRequirements(1, 1); @@ -64,7 +67,28 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) } } } - // warning if not all parameters are in the expression + + std::map parameterVars; + for (int i = 0; i < getProperty_variable_names().size(); ++i) { + std::string variableName = get_variable_names(i); + parameterVars[variableName] = getPropertyValue(i); + } + + // test to make sure all variables are there + try + { + m_parameterProg.evaluate(parameterVars); + } + catch (Lepton::Exception ex) + { + std::string msg = ex.what(); + std::string help = ""; + if (msg.compare(0, 30, "No value specified for variable")) { + help = " Use addParameter() to add a parameter for this variable, or" + " remove the variable from the expression for this goal."; + } + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Expression evaluate error: {}.{}", msg, help)); + } } double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { @@ -104,3 +128,11 @@ void MocoExpressionBasedParameterGoal::calcGoalImpl( values[0] = input.integral; } +void MocoExpressionBasedParameterGoal::printDescriptionImpl() const { + std::string msg; + msg += "MocoExpressionBasedParameterGoal expression: {}" + get_expression(); + for (int i = 0; i < getProperty_parameters().size(); ++i) { + msg += "\n\t" + get_variable_names(i) + ": " + get_parameters(i).getName(); + } + log_cout("{}", msg); +} diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index 48ea072efe..fd204be16d 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -34,6 +34,10 @@ class Model; number of MocoParameters that are combined into a single goal. The expression string should match the Lepton (lightweight expression parser) format. +Expressions can be any string that represents a mathematical expression, e.g., +"x*sqrt(y-8)". See Parser::getFunctionOperation for a full list of avilable +functions. + @ingroup mocogoal */ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoExpressionBasedParameterGoal, MocoGoal); @@ -47,11 +51,23 @@ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { : MocoGoal(std::move(name), weight) { constructProperties(); } + MocoExpressionBasedParameterGoal(std::string name, double weight, + std::string expression) : MocoGoal(std::move(name), weight) { + constructProperties(); + setExpression(expression); + } + /** Set the mathematical expression to minimize. Variable names should match + the names set with addParameter(). See header for explanation of + Expressions. */ void setExpression(std::string expression) { set_expression(expression); } + /** Add parameters with variable names that match the variables in the + expression string. All variables in the expression must have a corresponding + parameter, but parameters with variables that are not in the expression are + ignored. */ void addParameter(MocoParameter& parameter, std::string variableName) { updProperty_parameters().appendValue(parameter); updProperty_variable_names().appendValue(variableName); @@ -63,13 +79,18 @@ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { const IntegrandInput& input, SimTK::Real& integrand) const override; void calcGoalImpl( const GoalInput& input, SimTK::Vector& cost) const override; - //void printDescriptionImpl() const override; + void printDescriptionImpl() const override; private: void constructProperties(); + + /** Get the value of the property from its index in the property_refs vector. + This will use m_data_types to get the type, and if it is a Vec type, it uses + m_indices to get the element to return, both at the same index i.*/ + double getPropertyValue(int i) const; + OpenSim_DECLARE_PROPERTY(expression, std::string, "The expression string with variables q0-q9."); - // consider a mapping from variable names to parameters instead OpenSim_DECLARE_LIST_PROPERTY(parameters, MocoParameter, "MocoParameters to use in the expression."); OpenSim_DECLARE_LIST_PROPERTY(variable_names, std::string, @@ -85,7 +106,6 @@ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { mutable std::vector m_data_types; mutable std::vector m_indices; - double getPropertyValue(int i) const; }; } // namespace OpenSim diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index 628287ad6a..fd9d3464a3 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -1418,7 +1418,6 @@ TEST_CASE("MocoFrameDistanceConstraint de/serialization") { TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { SECTION("mass goal") { - // takes 20 seconds MocoStudy study; Model model = ModelFactory::createSlidingPointMass(); MocoProblem& mp = study.updProblem(); @@ -1427,7 +1426,8 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}); mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); - auto* parameter = mp.addParameter("sphere_mass", "body", "mass", MocoBounds(0, 10)); + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", + MocoBounds(0, 10)); auto* mass_goal = mp.addGoal(); mass_goal->setExpression("(q-4)^2"); mass_goal->addParameter(*parameter, "q"); @@ -1443,8 +1443,7 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { CHECK(sol.getParameter("sphere_mass") == Catch::Approx(4).epsilon(1e-2)); } - SECTION("Double mass goal") { - // takes 22 seconds + SECTION("two parameter mass goal") { MocoStudy study; auto model = createDoubleSlidingMassModel(); model->initSystem(); @@ -1456,8 +1455,10 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { mp.setStateInfo("/slider2/position/value", {-5, 5}, 1, {1.2, 1.3}); mp.setStateInfo("/slider2/position/speed", {-20, 20}); - auto* parameter = mp.addParameter("sphere_mass", "body", "mass", MocoBounds(0, 10)); - auto* parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", MocoBounds(0, 10)); + auto* parameter = mp.addParameter("sphere_mass", "body", "mass", + MocoBounds(0, 10)); + auto* parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", + MocoBounds(0, 10)); int total_weight = 7; auto* mass_goal = mp.addGoal(); mass_goal->setExpression(fmt::format("(p+q-{})^2", total_weight)); @@ -1469,8 +1470,8 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoTropterSolver") { MocoSolution sol = study.solve(); // 3.7 and 3.3 - CHECK(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass") == - Catch::Approx(total_weight).epsilon(1e-9)); + CHECK(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass") + == Catch::Approx(total_weight).epsilon(1e-9)); } } @@ -1511,8 +1512,6 @@ std::unique_ptr createOscillatorTwoSpringsModel() { } TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { - int N = 25; - MocoStudy study; study.setName("oscillator_spring_stiffnesses"); MocoProblem& mp = study.updProblem(); @@ -1521,20 +1520,20 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); - SECTION("single parameter") { - // Optimize a single stiffness value and apply to both springs. + SECTION("single parameter for two values") { + // create a parameter goal for the stiffness of both strings std::vector components = {"spring1", "spring2"}; auto* parameter = mp.addParameter("spring_stiffness", components, "stiffness", MocoBounds(0, 100)); - auto* spring_goal = mp.addGoal(); + // minimum is when p = 0.5*STIFFNESS spring_goal->setExpression(fmt::format("(p-{})^2", 0.5*STIFFNESS)); spring_goal->addParameter(*parameter, "p"); auto& ms = study.initCasADiSolver(); - ms.set_num_mesh_intervals(N); - ms.set_parameters_require_initsystem(false); // faster, still works with spring stiffness - + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); MocoSolution sol = study.solve(); CHECK(sol.getParameter("spring_stiffness") == @@ -1542,26 +1541,54 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { } SECTION("two parameters") { - // Optimize a single stiffness value and apply to both springs. + // create two parameters to include in the goal auto* parameter = mp.addParameter("spring_stiffness", "spring1", "stiffness", MocoBounds(0, 100)); auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", "stiffness", MocoBounds(0, 100)); - auto* spring_goal = mp.addGoal(); - spring_goal->setExpression(fmt::format("(p+q-{})^2", STIFFNESS)); + // minimum is when p + q = STIFFNESS + spring_goal->setExpression(fmt::format("square(p+q-{})", STIFFNESS)); spring_goal->addParameter(*parameter, "p"); spring_goal->addParameter(*parameter2, "q"); auto& ms = study.initCasADiSolver(); - ms.set_num_mesh_intervals(N); - ms.set_parameters_require_initsystem(false); // faster, still works with spring stiffness - + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); MocoSolution sol = study.solve(); - log_cout("{} {}", sol.getParameter("spring_stiffness") , sol.getParameter("spring2_stiffness")); + CHECK(sol.getParameter("spring_stiffness") + + sol.getParameter("spring2_stiffness") == + Catch::Approx(STIFFNESS).epsilon(1e-6)); + } + + SECTION("missing parameter") { + // create one parameter but have two variables + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal( + "stiffness", 1, fmt::format("(p+q-{})^2", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + + CHECK_THROWS(study.initCasADiSolver()); // missing q + } + + SECTION("missing parameter") { + // create two parameters for the goal, only one is used + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + + auto* spring_goal = mp.addGoal( + "stiffness", 1, fmt::format("(p-{})^2", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + // second parameter is ignored + spring_goal->addParameter(*parameter2, "a"); - CHECK(sol.getParameter("spring_stiffness") + sol.getParameter("spring2_stiffness") == - Catch::Approx(STIFFNESS).epsilon(1e-6)); + // shouldn't throw an error + study.initCasADiSolver(); } } From feddeb282431b421c1b7509cdec27930b47fdc4b Mon Sep 17 00:00:00 2001 From: Allison Date: Thu, 15 Aug 2024 10:04:51 -0700 Subject: [PATCH 11/35] fix exception reference and printImpl --- .../MocoExpressionBasedParameterGoal.cpp | 18 ++++++++---------- .../MocoExpressionBasedParameterGoal.h | 3 ++- OpenSim/Moco/Test/testMocoGoals.cpp | 1 + 3 files changed, 11 insertions(+), 11 deletions(-) diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index 10d351254d..6da99ecdce 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -34,7 +34,7 @@ void MocoExpressionBasedParameterGoal::constructProperties() { void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) const { - m_parameterProg = Lepton::Parser::parse(get_expression()).optimize().createProgram(); + m_program = Lepton::Parser::parse(get_expression()).optimize().createProgram(); setRequirements(1, 1); // store parameter property ref @@ -77,15 +77,15 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) // test to make sure all variables are there try { - m_parameterProg.evaluate(parameterVars); + m_program.evaluate(parameterVars); } - catch (Lepton::Exception ex) + catch (Lepton::Exception& ex) { std::string msg = ex.what(); std::string help = ""; if (msg.compare(0, 30, "No value specified for variable")) { - help = " Use addParameter() to add a parameter for this variable, or" - " remove the variable from the expression for this goal."; + help = " Use addParameter() to add a parameter for this variable, " + "or remove the variable from the expression for this goal."; } OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Expression evaluate error: {}.{}", msg, help)); } @@ -120,7 +120,7 @@ void MocoExpressionBasedParameterGoal::calcIntegrandImpl( std::string variableName = get_variable_names(i); parameterVars[variableName] = getPropertyValue(i); } - integrand = m_parameterProg.evaluate(parameterVars); + integrand = m_program.evaluate(parameterVars); } void MocoExpressionBasedParameterGoal::calcGoalImpl( @@ -129,10 +129,8 @@ void MocoExpressionBasedParameterGoal::calcGoalImpl( } void MocoExpressionBasedParameterGoal::printDescriptionImpl() const { - std::string msg; - msg += "MocoExpressionBasedParameterGoal expression: {}" + get_expression(); + log_cout(" expression: {}", get_expression()); for (int i = 0; i < getProperty_parameters().size(); ++i) { - msg += "\n\t" + get_variable_names(i) + ": " + get_parameters(i).getName(); + log_cout(" var {}: {}", get_variable_names(i), get_parameters(i).getName()); } - log_cout("{}", msg); } diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index fd204be16d..b4c6c8584a 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -96,7 +96,8 @@ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { OpenSim_DECLARE_LIST_PROPERTY(variable_names, std::string, "Variable names of the MocoParameters to use in the expression."); - mutable Lepton::ExpressionProgram m_parameterProg; + mutable Lepton::ExpressionProgram m_program; + // stores references to one property per parameter mutable std::vector> m_property_refs; enum DataType { Type_double, diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index fd9d3464a3..909f823469 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -1552,6 +1552,7 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { spring_goal->addParameter(*parameter, "p"); spring_goal->addParameter(*parameter2, "q"); + auto& ms = study.initCasADiSolver(); ms.set_num_mesh_intervals(25); // not requiring initsystem is faster, still works with spring stiffness From c1bd2ab3f4eafaf110d30b72c69ec4c6b9baa308 Mon Sep 17 00:00:00 2001 From: Allison Date: Thu, 15 Aug 2024 11:22:14 -0700 Subject: [PATCH 12/35] comments --- .../MocoExpressionBasedParameterGoal.cpp | 36 ++++++++++--------- .../MocoExpressionBasedParameterGoal.h | 14 +++++--- OpenSim/Moco/Test/testMocoGoals.cpp | 2 +- 3 files changed, 29 insertions(+), 23 deletions(-) diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index 6da99ecdce..cba55e2d57 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -21,7 +21,6 @@ #include #include #include - #include using namespace OpenSim; @@ -34,22 +33,22 @@ void MocoExpressionBasedParameterGoal::constructProperties() { void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) const { - m_program = Lepton::Parser::parse(get_expression()).optimize().createProgram(); + m_program = Lepton::Parser::parse(get_expression()).optimize() + .createProgram(); setRequirements(1, 1); - // store parameter property ref for (int i = 0; i < getProperty_parameters().size(); i++) { - std::string componentPath = get_parameters(i).getComponentPaths()[0]; // first one bc they should all be the same + // onylt taking the first one since they should all be the same value + std::string componentPath = get_parameters(i).getComponentPaths()[0]; const auto& component = model.getComponent(componentPath); - // Get component property. - const auto* ap = &component.getPropertyByName(get_parameters(i).getPropertyName()); + const auto* ap = &component.getPropertyByName( + get_parameters(i).getPropertyName()); m_property_refs.emplace_back(ap); // get the type of the property, and element OPENSIM_THROW_IF_FRMOBJ(ap->isListProperty(), Exception, "MocoParameter does not support list properties."); - // Type detection and property element value error checking. if (dynamic_cast*>(ap)) { m_data_types.emplace_back(Type_double); } else { @@ -68,15 +67,14 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) } } - std::map parameterVars; - for (int i = 0; i < getProperty_variable_names().size(); ++i) { - std::string variableName = get_variable_names(i); - parameterVars[variableName] = getPropertyValue(i); - } - // test to make sure all variables are there try { + std::map parameterVars; + for (int i = 0; i < getProperty_variable_names().size(); ++i) { + std::string variableName = get_variable_names(i); + parameterVars[variableName] = getPropertyValue(i); + } m_program.evaluate(parameterVars); } catch (Lepton::Exception& ex) @@ -87,7 +85,8 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) help = " Use addParameter() to add a parameter for this variable, " "or remove the variable from the expression for this goal."; } - OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Expression evaluate error: {}.{}", msg, help)); + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Expression evaluate error:" + " {}.{}", msg, help)); } } @@ -102,11 +101,13 @@ double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { int elt = m_indices[i]; if (m_data_types[i] == Type_Vec3) { - return static_cast*>(propRef.get())->getValue()[elt]; + return static_cast*>(propRef.get()) + ->getValue()[elt]; } if (m_data_types[i] == Type_Vec6) { - return static_cast*>(propRef.get())->getValue()[elt]; + return static_cast*>(propRef.get()) + ->getValue()[elt]; } OPENSIM_THROW_FRMOBJ(Exception, "Properties not of a recognized type."); @@ -131,6 +132,7 @@ void MocoExpressionBasedParameterGoal::calcGoalImpl( void MocoExpressionBasedParameterGoal::printDescriptionImpl() const { log_cout(" expression: {}", get_expression()); for (int i = 0; i < getProperty_parameters().size(); ++i) { - log_cout(" var {}: {}", get_variable_names(i), get_parameters(i).getName()); + log_cout(" var {}: {}", get_variable_names(i), + get_parameters(i).getName()); } } diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index b4c6c8584a..c92cdb8796 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -20,10 +20,9 @@ #include "MocoGoal.h" #include "OpenSim/Moco/MocoParameter.h" -#include - #include #include +#include #include #include @@ -35,8 +34,12 @@ number of MocoParameters that are combined into a single goal. The expression string should match the Lepton (lightweight expression parser) format. Expressions can be any string that represents a mathematical expression, e.g., -"x*sqrt(y-8)". See Parser::getFunctionOperation for a full list of avilable -functions. +"x*sqrt(y-8)". Expressions can contain variables, constants, operations, +parentheses, commas, spaces, and scientific e notation. The full list of +operations (also in Lepton::Operation) is: +sqrt, exp, log, sin, cos, sec, csc, tan, cot, asin, acos, atan, sinh, cosh, +tanh, erf, erfc, step, delta, square, cube, recip, min, max, abs, as well as ++, -, *, /, and ^. @ingroup mocogoal */ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { @@ -44,7 +47,8 @@ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { public: MocoExpressionBasedParameterGoal() { constructProperties(); } - MocoExpressionBasedParameterGoal(std::string name) : MocoGoal(std::move(name)) { + MocoExpressionBasedParameterGoal(std::string name) + : MocoGoal(std::move(name)) { constructProperties(); } MocoExpressionBasedParameterGoal(std::string name, double weight) diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index 909f823469..b877d54a74 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -1548,7 +1548,7 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { "stiffness", MocoBounds(0, 100)); auto* spring_goal = mp.addGoal(); // minimum is when p + q = STIFFNESS - spring_goal->setExpression(fmt::format("square(p+q-{})", STIFFNESS)); + spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); spring_goal->addParameter(*parameter, "p"); spring_goal->addParameter(*parameter2, "q"); From f53f90d266c09db411803bedfd19448e1935a7c4 Mon Sep 17 00:00:00 2001 From: Allison Date: Thu, 15 Aug 2024 11:24:41 -0700 Subject: [PATCH 13/35] changelog --- CHANGELOG_MOCO.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/CHANGELOG_MOCO.md b/CHANGELOG_MOCO.md index 130b9afcbf..8374c1d95d 100644 --- a/CHANGELOG_MOCO.md +++ b/CHANGELOG_MOCO.md @@ -3,6 +3,9 @@ Moco Change Log 1.3.1 ----- +- 2024-08-15: Added `MocoExpressionBasedParameterGoal` to enable minimizing any arithmetic expression + of parameter values. + - 2024-07-26: Added `MocoStateBoundConstraint` and `MocoOutputBoundConstraint` to enable bounding state variables or output values by one or two `Function`s, similar to `MocoControlBoundConstraint`. From 4527e19c9c638785060d255ad50dbccf2c3c3fab Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 15 Aug 2024 12:59:49 -0700 Subject: [PATCH 14/35] Add Output for ExpressionBasedBushingForce --- .../Model/ExpressionBasedBushingForce.cpp | 33 ++++++++++++++----- .../Model/ExpressionBasedBushingForce.h | 18 +++++++++- .../Model/ExpressionBasedCoordinateForce.h | 4 +-- .../Model/ExpressionBasedPointToPointForce.h | 2 +- 4 files changed, 44 insertions(+), 13 deletions(-) diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp index 8bfd4c3b2f..16f418e9b7 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp @@ -215,6 +215,13 @@ void ExpressionBasedBushingForce::extendFinalizeFromProperties() } } +void ExpressionBasedBushingForce::extendAddToSystem( + SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); + this->_bushingForceCV = addCacheVariable("bushing_force", SimTK::Vec6(0), + SimTK::Stage::Velocity); +} + /** Set the expression for the Mx function and create it's lepton program */ void ExpressionBasedBushingForce::setMxExpression(std::string expression) { @@ -309,6 +316,22 @@ SimTK::Vec6 ExpressionBasedBushingForce:: return -_dampingMatrix * dqdot; } +void ExpressionBasedBushingForce::calcBushingForce( + const SimTK::State& s) const { + if (isCacheVariableValid(s, _bushingForceCV)) { + return; + } + + setCacheVariableValue(s, _bushingForceCV, + calcStiffnessForce(s) + calcDampingForce(s)); +} + +const SimTK::Vec6& ExpressionBasedBushingForce::getBushingForce( + const SimTK::State& s) const { + calcBushingForce(s); + return getCacheVariableValue(s, _bushingForceCV); +} + /* Compute the force contribution to the system and add in to appropriate * bodyForce and/or system generalizedForce. */ @@ -316,17 +339,9 @@ void ExpressionBasedBushingForce::computeForce(const SimTK::State& s, SimTK::Vector_& bodyForces, SimTK::Vector& generalizedForces) const { - // stiffness force - Vec6 fk = calcStiffnessForce(s); - // damping force - Vec6 fv = calcDampingForce(s); - - // total bushing force in the internal basis of the deflection (dq) - Vec6 f = fk + fv; - // convert internal forces to spatial and add then add to system // physical (body) forces - addInPhysicalForcesFromInternal(s, f, bodyForces); + addInPhysicalForcesFromInternal(s, getBushingForce(s), bodyForces); } //============================================================================= diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h index 2c3ace22bb..0ff7fe75d4 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h @@ -93,6 +93,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); OpenSim_DECLARE_PROPERTY(translational_damping, SimTK::Vec3, "Damping parameters resisting translational deflection (delta_dot) ."); +//============================================================================== +// PROPERTIES +//============================================================================== + OpenSim_DECLARE_OUTPUT(bushing_force, SimTK::Vec6, getBushingForce, + SimTK::Stage::Velocity); + //============================================================================== // PUBLIC METHODS //============================================================================== @@ -214,6 +220,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); theta_x, theta_y, theta_z, delta_x, delta_y, delta_z **/ std::string getFzExpression() { return get_Fz_expression(); } + /** Get the total bushing force. This is the sum of the stiffness and damping + force contributions. **/ + const SimTK::Vec6& getBushingForce(const SimTK::State& state) const; + //-------------------------------------------------------------------------- // COMPUTATION //-------------------------------------------------------------------------- @@ -226,7 +236,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); function of the deflection rate between the bushing frames. It is the force on frame2 from frame1 in the basis of the deflection rate (dqdot).*/ SimTK::Vec6 calcDampingForce(const SimTK::State& state) const; - //-------------------------------------------------------------------------- // Reporting @@ -266,15 +275,22 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); // Implement ModelComponent interface. //-------------------------------------------------------------------------- void extendFinalizeFromProperties() override; + void extendAddToSystem(SimTK::MultibodySystem& system) const override; void setNull(); void constructProperties(); + /** Calculate the total bushing force. This is the sum of the stiffness and + damping force contributions. */ + void calcBushingForce(const SimTK::State& state) const; + SimTK::Mat66 _dampingMatrix{ 0.0 }; // parser programs for efficiently evaluating the expressions Lepton::ExpressionProgram MxProg, MyProg, MzProg, FxProg, FyProg, FzProg; + mutable CacheVariable _bushingForceCV; + //============================================================================== }; // END of class ExpressionBasedBushingForce //============================================================================== diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h index 694bb32525..d246015098 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h @@ -44,8 +44,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); //============================================================================== // OUTPUTS //============================================================================== - OpenSim_DECLARE_OUTPUT(force, double, getForceMagnitude, - SimTK::Stage::Dynamics); + OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude, + SimTK::Stage::Velocity); //============================================================================== // PUBLIC METHODS //============================================================================== diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h index b913561c72..d67a7430f2 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h @@ -75,7 +75,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); // OUTPUTS //============================================================================== OpenSim_DECLARE_OUTPUT(force, double, getForceMagnitude, - SimTK::Stage::Dynamics); + SimTK::Stage::Velocity); //============================================================================== // PUBLIC METHODS From 31ea6b55ade87787b0294e430b64cac7ea1e3b4c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 15 Aug 2024 13:12:37 -0700 Subject: [PATCH 15/35] Add test for Output for ExpressionBasedBushingForce --- CHANGELOG.md | 2 +- .../Model/ExpressionBasedBushingForce.cpp | 4 ++-- .../Model/ExpressionBasedCoordinateForce.cpp | 2 +- OpenSim/Simulation/Test/testForces.cpp | 15 +++++++++++++-- 4 files changed, 17 insertions(+), 6 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0b938cd137..c1ffd6fca4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -75,7 +75,7 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from occurring in the output. (#3867) -- Added `Output`s to `ExpressionBasedCoordinateForce` and `ExpressionBasedPointToPointForce` for accessing force values. (#3872) +- Added `Output`s to `ExpressionBasedCoordinateForce`, `ExpressionBasedPointToPointForce`, and `ExpressionBasedBushingForce` for accessing force values. (#3872) v4.5 diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp index 16f418e9b7..66f6b00b71 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp @@ -382,7 +382,7 @@ OpenSim::Array ExpressionBasedBushingForce:: SpatialVec F_GF( Vec3(0.0),Vec3(0.0) ); // total bushing force in the internal basis of the deflection (dq) - Vec6 f = calcStiffnessForce(s) + calcDampingForce(s); + const Vec6& f = getBushingForce(s); convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM); @@ -441,7 +441,7 @@ void ExpressionBasedBushingForce::generateDecorations SpatialVec F_GF(Vec3(0.0), Vec3(0.0)); // total bushing force in the internal basis of the deflection (dq) - Vec6 f = calcStiffnessForce(s) + calcDampingForce(s); + const Vec6& f = getBushingForce(s); convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM); diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp index f05bf5403d..c54dd59fdf 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.cpp @@ -53,7 +53,7 @@ ExpressionBasedCoordinateForce::ExpressionBasedCoordinateForce( setExpression(expression); } -// Set the expression for the force function and create it's lepton program +// Set the expression for the force function and create its lepton program void ExpressionBasedCoordinateForce::setExpression(const string& expression) { set_expression(expression); diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index e5c090920f..417765143a 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -34,8 +34,9 @@ // 8. ExternalForce // 9. PathSpring // 10. ExpressionBasedCoordinateForce -// 10. ExpressionBasedPointToPointForce -// 11. Blankevoort1991Ligament +// 11. ExpressionBasedPointToPointForce +// 12. ExpressionBasedBushingForce +// 13. Blankevoort1991Ligament // // Add tests here as Forces are added to OpenSim // @@ -929,6 +930,11 @@ TEST_CASE("testExpressionBasedBushingForceTranslational") { // check analytical force corresponds to the force on the ball // in the Y direction, index = 7 ASSERT_EQUAL(analytical_force, model_force[7], 2e-4); + + // check that the force from the Output is correct + SimTK::Vec6 output_force = + spring.getOutputValue(osim_state, "bushing_force"); + ASSERT_EQUAL(analytical_force, output_force[4], 2e-4); } manager.getStateStorage().print( @@ -1060,6 +1066,11 @@ TEST_CASE("testExpressionBasedBushingForceRotational") { // check analytical moment corresponds to the moment on the ball // in the Y direction, index = 4 ASSERT_EQUAL(analytical_moment, model_forces[4], 2e-4); + + // check that the force from the Output is correct + SimTK::Vec6 output_force = + spring.getOutputValue(osim_state, "bushing_force"); + ASSERT_EQUAL(analytical_moment, output_force[1], 2e-4); } manager.getStateStorage().print( From 5d4595b83c23ef08dfe3dff467db16be90044ea3 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 15 Aug 2024 13:36:48 -0700 Subject: [PATCH 16/35] Use correct Output names --- OpenSim/Simulation/Test/testForces.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 417765143a..c602382fa6 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -192,7 +192,7 @@ TEST_CASE("testExpressionBasedCoordinateForce") { double analytical_force = -10*ball_h - 5*ball_h_dot; double model_force = spring->getForceMagnitude(osim_state); double output_force = - spring->getOutputValue(osim_state, "force"); + spring->getOutputValue(osim_state, "force_magnitude"); ASSERT_EQUAL(analytical_force, model_force, 1e-5); ASSERT_EQUAL(analytical_force, output_force, 1e-5); } @@ -277,7 +277,8 @@ TEST_CASE("testExpressionBasedPointToPointForce") { // Now check that the force reported by spring double model_force = p2pForce->getForceMagnitude(state); - double output_force = p2pForce->getOutputValue(state, "force"); + double output_force = + p2pForce->getOutputValue(state, "force_magnitude"); // Save the forces // reporter->getForceStorage().print("path_spring_forces.mot"); From 83b890b6c5e7a0269087ddac9879bede46da5794 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 15 Aug 2024 15:01:13 -0700 Subject: [PATCH 17/35] Fix bushing force test --- OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h | 2 +- OpenSim/Simulation/Test/testForces.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h index d67a7430f2..845f4e4847 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h @@ -74,7 +74,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); //============================================================================== // OUTPUTS //============================================================================== - OpenSim_DECLARE_OUTPUT(force, double, getForceMagnitude, + OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude, SimTK::Stage::Velocity); //============================================================================== diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index c602382fa6..51fc9096e2 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -1071,7 +1071,7 @@ TEST_CASE("testExpressionBasedBushingForceRotational") { // check that the force from the Output is correct SimTK::Vec6 output_force = spring.getOutputValue(osim_state, "bushing_force"); - ASSERT_EQUAL(analytical_moment, output_force[1], 2e-4); + ASSERT_EQUAL(analytical_moment, -output_force[1], 2e-4); } manager.getStateStorage().print( From 7f3a19455d3d76a1f7025c990dfb282e1a41ee34 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 15 Aug 2024 16:26:25 -0700 Subject: [PATCH 18/35] Use the correct SimTK::Stage for all Outputs --- .../Model/ExpressionBasedBushingForce.cpp | 29 +++++++++---------- .../Model/ExpressionBasedBushingForce.h | 9 +++--- .../Model/ExpressionBasedCoordinateForce.h | 2 +- .../Model/ExpressionBasedPointToPointForce.h | 2 +- 4 files changed, 20 insertions(+), 22 deletions(-) diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp index 66f6b00b71..1f18cdbbe6 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp @@ -276,6 +276,12 @@ void ExpressionBasedBushingForce::setFzExpression(std::string expression) set_Fz_expression(expression); FzProg = Lepton::Parser::parse(expression).optimize().createProgram(); } + +const SimTK::Vec6& ExpressionBasedBushingForce::getBushingForce( + const SimTK::State& s) const { + return getCacheVariableValue(s, _bushingForceCV); +} + //============================================================================= // COMPUTATION //============================================================================= @@ -316,20 +322,11 @@ SimTK::Vec6 ExpressionBasedBushingForce:: return -_dampingMatrix * dqdot; } -void ExpressionBasedBushingForce::calcBushingForce( +SimTK::Vec6 ExpressionBasedBushingForce::calcBushingForce( const SimTK::State& s) const { - if (isCacheVariableValid(s, _bushingForceCV)) { - return; - } - - setCacheVariableValue(s, _bushingForceCV, - calcStiffnessForce(s) + calcDampingForce(s)); -} - -const SimTK::Vec6& ExpressionBasedBushingForce::getBushingForce( - const SimTK::State& s) const { - calcBushingForce(s); - return getCacheVariableValue(s, _bushingForceCV); + Vec6 f = calcStiffnessForce(s) + calcDampingForce(s); + setCacheVariableValue(s, _bushingForceCV, f); + return f; } @@ -341,7 +338,7 @@ void ExpressionBasedBushingForce::computeForce(const SimTK::State& s, { // convert internal forces to spatial and add then add to system // physical (body) forces - addInPhysicalForcesFromInternal(s, getBushingForce(s), bodyForces); + addInPhysicalForcesFromInternal(s, calcBushingForce(s), bodyForces); } //============================================================================= @@ -382,7 +379,7 @@ OpenSim::Array ExpressionBasedBushingForce:: SpatialVec F_GF( Vec3(0.0),Vec3(0.0) ); // total bushing force in the internal basis of the deflection (dq) - const Vec6& f = getBushingForce(s); + Vec6 f = calcBushingForce(s); convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM); @@ -441,7 +438,7 @@ void ExpressionBasedBushingForce::generateDecorations SpatialVec F_GF(Vec3(0.0), Vec3(0.0)); // total bushing force in the internal basis of the deflection (dq) - const Vec6& f = getBushingForce(s); + Vec6 f = calcBushingForce(s); convertInternalForceToForcesOnFrames(s, f, F_GF, F_GM); diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h index 0ff7fe75d4..2f79e410aa 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h @@ -94,10 +94,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); "Damping parameters resisting translational deflection (delta_dot) ."); //============================================================================== -// PROPERTIES +// OUTPUTS //============================================================================== OpenSim_DECLARE_OUTPUT(bushing_force, SimTK::Vec6, getBushingForce, - SimTK::Stage::Velocity); + SimTK::Stage::Dynamics); //============================================================================== // PUBLIC METHODS @@ -221,7 +221,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); std::string getFzExpression() { return get_Fz_expression(); } /** Get the total bushing force. This is the sum of the stiffness and damping - force contributions. **/ + force contributions. Note, computeForce must be evaluated first, + and this is done automatically if the system is realized to Dynamics. */ const SimTK::Vec6& getBushingForce(const SimTK::State& state) const; //-------------------------------------------------------------------------- @@ -282,7 +283,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); /** Calculate the total bushing force. This is the sum of the stiffness and damping force contributions. */ - void calcBushingForce(const SimTK::State& state) const; + SimTK::Vec6 calcBushingForce(const SimTK::State& state) const; SimTK::Mat66 _dampingMatrix{ 0.0 }; diff --git a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h index d246015098..e6e0b3313a 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedCoordinateForce.h @@ -45,7 +45,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedCoordinateForce, Force); // OUTPUTS //============================================================================== OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude, - SimTK::Stage::Velocity); + SimTK::Stage::Dynamics); //============================================================================== // PUBLIC METHODS //============================================================================== diff --git a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h index 845f4e4847..67a3929d13 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedPointToPointForce.h @@ -75,7 +75,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedPointToPointForce, Force); // OUTPUTS //============================================================================== OpenSim_DECLARE_OUTPUT(force_magnitude, double, getForceMagnitude, - SimTK::Stage::Velocity); + SimTK::Stage::Dynamics); //============================================================================== // PUBLIC METHODS From 5c529a123a71e86620877d0a512280f798ed9a46 Mon Sep 17 00:00:00 2001 From: Allison Date: Thu, 15 Aug 2024 17:14:19 -0700 Subject: [PATCH 19/35] no more integral, small edits --- .../MocoExpressionBasedParameterGoal.cpp | 41 +++++++++---------- .../MocoExpressionBasedParameterGoal.h | 41 +++++++++++++------ OpenSim/Moco/MocoParameter.h | 3 +- OpenSim/Moco/Test/testMocoGoals.cpp | 11 +++-- 4 files changed, 54 insertions(+), 42 deletions(-) diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index cba55e2d57..4e4a587d37 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -1,5 +1,5 @@ /* -------------------------------------------------------------------------- * -* OpenSim: MocoExpressionBasedParameterGoal.h * + * OpenSim: MocoExpressionBasedParameterGoal.cpp * * -------------------------------------------------------------------------- * * Copyright (c) 2024 Stanford University and the Authors * * * @@ -33,32 +33,32 @@ void MocoExpressionBasedParameterGoal::constructProperties() { void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) const { + if (get_expression() == "") { + log_warn("The expression has not been set."); + } m_program = Lepton::Parser::parse(get_expression()).optimize() .createProgram(); - setRequirements(1, 1); + setRequirements(0, 1, SimTK::Stage::Instance); for (int i = 0; i < getProperty_parameters().size(); i++) { - // onylt taking the first one since they should all be the same value + // only taking the first one since they should all be the same value std::string componentPath = get_parameters(i).getComponentPaths()[0]; const auto& component = model.getComponent(componentPath); const auto* ap = &component.getPropertyByName( get_parameters(i).getPropertyName()); m_property_refs.emplace_back(ap); - // get the type of the property, and element - OPENSIM_THROW_IF_FRMOBJ(ap->isListProperty(), Exception, - "MocoParameter does not support list properties."); - + // get the type and element of the property if (dynamic_cast*>(ap)) { m_data_types.emplace_back(Type_double); } else { if (dynamic_cast*>(ap)) { m_data_types.emplace_back(Type_Vec3); - m_indices.emplace_back(get_parameters(i).getElement()); + m_indices.emplace_back(get_parameters(i).getPropertyElement()); } else if (dynamic_cast*>(ap)) { m_data_types.emplace_back(Type_Vec6); - m_indices.emplace_back(get_parameters(i).getElement()); + m_indices.emplace_back(get_parameters(i).getPropertyElement()); } else { OPENSIM_THROW_FRMOBJ(Exception, @@ -91,8 +91,9 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) } double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { - OPENSIM_THROW_IF_FRMOBJ((int) m_property_refs.size() < i + 1, Exception, - "Doesn't have that many parameters.") + OPENSIM_THROW_IF_FRMOBJ(static_cast(m_property_refs.size()) <= i, Exception, + "Property index is out of bounds.") + const auto& propRef = m_property_refs[i]; if (m_data_types[i] == Type_double) { @@ -110,29 +111,25 @@ double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { ->getValue()[elt]; } - OPENSIM_THROW_FRMOBJ(Exception, "Properties not of a recognized type."); -} + OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Property at index {} is not of" + " a recognized type.")); -void MocoExpressionBasedParameterGoal::calcIntegrandImpl( - const IntegrandInput& input, double& integrand) const { +} +void MocoExpressionBasedParameterGoal::calcGoalImpl( + const GoalInput& input, SimTK::Vector& values) const { std::map parameterVars; for (int i = 0; i < getProperty_variable_names().size(); ++i) { std::string variableName = get_variable_names(i); parameterVars[variableName] = getPropertyValue(i); } - integrand = m_program.evaluate(parameterVars); -} - -void MocoExpressionBasedParameterGoal::calcGoalImpl( - const GoalInput& input, SimTK::Vector& values) const { - values[0] = input.integral; + values[0] = m_program.evaluate(parameterVars); } void MocoExpressionBasedParameterGoal::printDescriptionImpl() const { log_cout(" expression: {}", get_expression()); for (int i = 0; i < getProperty_parameters().size(); ++i) { - log_cout(" var {}: {}", get_variable_names(i), + log_cout(" variable {}: {}", get_variable_names(i), get_parameters(i).getName()); } } diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index c92cdb8796..1d19a73930 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -20,11 +20,8 @@ #include "MocoGoal.h" #include "OpenSim/Moco/MocoParameter.h" -#include -#include #include #include -#include namespace OpenSim { class Model; @@ -33,6 +30,8 @@ class Model; number of MocoParameters that are combined into a single goal. The expression string should match the Lepton (lightweight expression parser) format. +# Creating Expressions + Expressions can be any string that represents a mathematical expression, e.g., "x*sqrt(y-8)". Expressions can contain variables, constants, operations, parentheses, commas, spaces, and scientific e notation. The full list of @@ -41,6 +40,25 @@ sqrt, exp, log, sin, cos, sec, csc, tan, cot, asin, acos, atan, sinh, cosh, tanh, erf, erfc, step, delta, square, cube, recip, min, max, abs, as well as +, -, *, /, and ^. +# Examples + +@code +// create two parameters to include in the goal +MocoStudy study; +MocoProblem& mp = study.updProblem(); +mp.setModel(createOscillatorTwoSpringsModel()); +mp.setTimeBounds(0, FINAL_TIME); +auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); +auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); +auto* spring_goal = mp.addGoal(); +// minimum is when p + q = STIFFNESS +spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); +spring_goal->addParameter(*parameter, "p"); +spring_goal->addParameter(*parameter2, "q"); +@endcode + @ingroup mocogoal */ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoExpressionBasedParameterGoal, MocoGoal); @@ -58,31 +76,30 @@ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { MocoExpressionBasedParameterGoal(std::string name, double weight, std::string expression) : MocoGoal(std::move(name), weight) { constructProperties(); - setExpression(expression); + set_expression(std::move(expression)); } /** Set the mathematical expression to minimize. Variable names should match - the names set with addParameter(). See header for explanation of - Expressions. */ + the names set with addParameter(). See Creating Expressions above for an + explanation of Expressions. */ void setExpression(std::string expression) { - set_expression(expression); + set_expression(std::move(expression)); } /** Add parameters with variable names that match the variables in the expression string. All variables in the expression must have a corresponding parameter, but parameters with variables that are not in the expression are ignored. */ - void addParameter(MocoParameter& parameter, std::string variableName) { - updProperty_parameters().appendValue(parameter); - updProperty_variable_names().appendValue(variableName); + void addParameter(const MocoParameter& parameter, std::string variableName) { + append_parameters(parameter); + append_variable_names(std::move(variableName)); } protected: void initializeOnModelImpl(const Model& model) const override; - void calcIntegrandImpl( - const IntegrandInput& input, SimTK::Real& integrand) const override; void calcGoalImpl( const GoalInput& input, SimTK::Vector& cost) const override; + bool getSupportsEndpointConstraintImpl() const override { return true; } void printDescriptionImpl() const override; private: diff --git a/OpenSim/Moco/MocoParameter.h b/OpenSim/Moco/MocoParameter.h index d41abeda38..aaa47c3e53 100644 --- a/OpenSim/Moco/MocoParameter.h +++ b/OpenSim/Moco/MocoParameter.h @@ -23,7 +23,6 @@ #include #include #include -#include namespace OpenSim { @@ -142,7 +141,7 @@ class OSIMMOCO_API MocoParameter : public Object { { set_property_name(propertyName); } void appendComponentPath(const std::string& componentPath) { append_component_paths(componentPath); } - int getElement() const { + int getPropertyElement() const { return get_property_element(); } diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index b877d54a74..833606229a 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -16,9 +16,6 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -#include "Testing.h" -#include - #include #include #include @@ -29,6 +26,9 @@ #include #include +#include "Testing.h" +#include + using Catch::Approx; using Catch::Matchers::ContainsSubstring; @@ -1576,7 +1576,7 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { CHECK_THROWS(study.initCasADiSolver()); // missing q } - SECTION("missing parameter") { + SECTION("extra parameter") { // create two parameters for the goal, only one is used auto* parameter = mp.addParameter("spring_stiffness", "spring1", "stiffness", MocoBounds(0, 100)); @@ -1589,7 +1589,6 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { // second parameter is ignored spring_goal->addParameter(*parameter2, "a"); - // shouldn't throw an error - study.initCasADiSolver(); + CHECK_NOTHROW(study.initCasADiSolver()); } } From 19985abe54fbf81bdd3e8b5fae1980b26528b5dd Mon Sep 17 00:00:00 2001 From: Allison Date: Fri, 16 Aug 2024 10:32:23 -0700 Subject: [PATCH 20/35] example in header and endpoint test --- .../MocoExpressionBasedParameterGoal.h | 18 ++++++--------- OpenSim/Moco/Test/testMocoGoals.cpp | 23 +++++++++++++++++++ 2 files changed, 30 insertions(+), 11 deletions(-) diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index 1d19a73930..03cc164af9 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -43,20 +43,16 @@ tanh, erf, erfc, step, delta, square, cube, recip, min, max, abs, as well as # Examples @code -// create two parameters to include in the goal -MocoStudy study; -MocoProblem& mp = study.updProblem(); -mp.setModel(createOscillatorTwoSpringsModel()); -mp.setTimeBounds(0, FINAL_TIME); -auto* parameter = mp.addParameter("spring_stiffness", "spring1", - "stiffness", MocoBounds(0, 100)); -auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", - "stiffness", MocoBounds(0, 100)); +auto* spring1_parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); +auto* spring2_parameter = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); auto* spring_goal = mp.addGoal(); +double STIFFNESS = 100.0; // minimum is when p + q = STIFFNESS spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); -spring_goal->addParameter(*parameter, "p"); -spring_goal->addParameter(*parameter2, "q"); +spring_goal->addParameter(*spring1_parameter, "p"); +spring_goal->addParameter(*spring2_parameter, "q"); @endcode @ingroup mocogoal */ diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index 833606229a..5234de05b0 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -1591,4 +1591,27 @@ TEST_CASE("MocoExpressionBasedParameterGoal - MocoCasADiSolver") { CHECK_NOTHROW(study.initCasADiSolver()); } + + SECTION("endpoint goal") { + auto* parameter = mp.addParameter("spring_stiffness", "spring1", + "stiffness", MocoBounds(0, 100)); + auto* parameter2 = mp.addParameter("spring2_stiffness", "spring2", + "stiffness", MocoBounds(0, 100)); + auto* spring_goal = mp.addGoal(); + spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS)); + spring_goal->addParameter(*parameter, "p"); + spring_goal->addParameter(*parameter2, "q"); + // set as endpoint constraint + spring_goal->setMode("endpoint_constraint"); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(25); + // not requiring initsystem is faster, still works with spring stiffness + ms.set_parameters_require_initsystem(false); + MocoSolution sol = study.solve(); + + CHECK(sol.getParameter("spring_stiffness") + + sol.getParameter("spring2_stiffness") == + Catch::Approx(STIFFNESS).epsilon(1e-6)); + } } From a6d9677b8637d8ee063f52051d899d9ff4a48127 Mon Sep 17 00:00:00 2001 From: Allison Date: Fri, 16 Aug 2024 12:04:18 -0700 Subject: [PATCH 21/35] python test --- Bindings/Python/tests/test_moco.py | 48 +++++++++++++++++++ .../MocoExpressionBasedParameterGoal.cpp | 20 ++------ .../MocoExpressionBasedParameterGoal.h | 5 +- 3 files changed, 56 insertions(+), 17 deletions(-) diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index eda1bf7f33..ce7d005cb2 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -35,6 +35,26 @@ def createSlidingMassModel(): return model +def createDoubleSlidingMassModel(): + model = createSlidingMassModel() + body = osim.Body("body2", 10.0, osim.Vec3(0), osim.Inertia(0)) + model.addComponent(body) + + joint = osim.SliderJoint("slider2", model.getGround(), body) + coord = joint.updCoordinate(osim.SliderJoint.Coord_TranslationX) + coord.setName("position"); + model.addComponent(joint); + + actu = osim.CoordinateActuator() + actu.setCoordinate(coord) + actu.setName("actuator2") + actu.setOptimalForce(1) + model.addComponent(actu) + + model.finalizeConnections() + return model + + class TestSwigAddtlInterface(unittest.TestCase): def test_bounds(self): model = osim.Model() @@ -391,3 +411,31 @@ def test_changing_costs(self): # Change the weights of the costs. effort.setWeight(0.1) assert(study.solve().getFinalTime() < 0.8 * finalTime0) + + def test_expression_based_parameter_goal(self): + study = osim.MocoStudy() + #model->initSystem(); + mp = study.updProblem() + mp.setModel(createDoubleSlidingMassModel()) + mp.setTimeBounds(0, 1) + mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}) + mp.setStateInfo("/slider/position/speed", {-20, 20}) + mp.setStateInfo("/slider2/position/value", {-5, 5}, 1, {1.2, 1.3}) + mp.setStateInfo("/slider2/position/speed", {-20, 20}) + + parameter = mp.addParameter("sphere_mass", "body", "mass", + osim.MocoBounds(0, 10)) + parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", + osim.MocoBounds(0, 10)) + total_weight = 7 + mass_goal = mp.addGoal() + mass_goal.setExpression(f"(p+q-{total_weight})^2") + mass_goal.addParameter(parameter, "p") + mass_goal.addParameter(parameter2, "q") + + ms = study.initTropterSolver() + ms.set_num_mesh_intervals(25) + sol = study.solve() + + self.assertAlmostEqual(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass"), + total_weight) \ No newline at end of file diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp index 4e4a587d37..499379bb07 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp @@ -33,9 +33,8 @@ void MocoExpressionBasedParameterGoal::constructProperties() { void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) const { - if (get_expression() == "") { - log_warn("The expression has not been set."); - } + OPENSIM_THROW_IF_FRMOBJ(get_expression() == "", Exception, + "The expression has not been set. Use setExpression().") m_program = Lepton::Parser::parse(get_expression()).optimize() .createProgram(); setRequirements(0, 1, SimTK::Stage::Instance); @@ -72,8 +71,7 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) { std::map parameterVars; for (int i = 0; i < getProperty_variable_names().size(); ++i) { - std::string variableName = get_variable_names(i); - parameterVars[variableName] = getPropertyValue(i); + parameterVars[get_variable_names(i)] = getPropertyValue(i); } m_program.evaluate(parameterVars); } @@ -91,37 +89,29 @@ void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model) } double MocoExpressionBasedParameterGoal::getPropertyValue(int i) const { - OPENSIM_THROW_IF_FRMOBJ(static_cast(m_property_refs.size()) <= i, Exception, - "Property index is out of bounds.") - + OPENSIM_ASSERT_FRMOBJ(m_property_refs.size() > i); const auto& propRef = m_property_refs[i]; - if (m_data_types[i] == Type_double) { return static_cast*>(propRef.get())->getValue(); } - int elt = m_indices[i]; if (m_data_types[i] == Type_Vec3) { return static_cast*>(propRef.get()) ->getValue()[elt]; } - if (m_data_types[i] == Type_Vec6) { return static_cast*>(propRef.get()) ->getValue()[elt]; } - OPENSIM_THROW_FRMOBJ(Exception, fmt::format("Property at index {} is not of" " a recognized type.")); - } void MocoExpressionBasedParameterGoal::calcGoalImpl( const GoalInput& input, SimTK::Vector& values) const { std::map parameterVars; for (int i = 0; i < getProperty_variable_names().size(); ++i) { - std::string variableName = get_variable_names(i); - parameterVars[variableName] = getPropertyValue(i); + parameterVars[get_variable_names(i)] = getPropertyValue(i); } values[0] = m_program.evaluate(parameterVars); } diff --git a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h index 03cc164af9..804eac87e6 100644 --- a/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h @@ -76,8 +76,9 @@ class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal { } /** Set the mathematical expression to minimize. Variable names should match - the names set with addParameter(). See Creating Expressions above for an - explanation of Expressions. */ + the names set with addParameter(). See "Creating Expressions" in the class + documentation above for an explanation of how to create expressions. + */ void setExpression(std::string expression) { set_expression(std::move(expression)); } From 915de4c21a8556eada5843c90754b8344864a1c9 Mon Sep 17 00:00:00 2001 From: Allison Date: Fri, 16 Aug 2024 12:12:50 -0700 Subject: [PATCH 22/35] fix test --- Bindings/Python/tests/test_moco.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index ce7d005cb2..2048005af7 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -52,6 +52,7 @@ def createDoubleSlidingMassModel(): model.addComponent(actu) model.finalizeConnections() + model.initSystem() return model @@ -414,7 +415,6 @@ def test_changing_costs(self): def test_expression_based_parameter_goal(self): study = osim.MocoStudy() - #model->initSystem(); mp = study.updProblem() mp.setModel(createDoubleSlidingMassModel()) mp.setTimeBounds(0, 1) @@ -428,7 +428,7 @@ def test_expression_based_parameter_goal(self): parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", osim.MocoBounds(0, 10)) total_weight = 7 - mass_goal = mp.addGoal() + mass_goal = mp.addGoal(osim.MocoExpressionBasedParameterGoal()) mass_goal.setExpression(f"(p+q-{total_weight})^2") mass_goal.addParameter(parameter, "p") mass_goal.addParameter(parameter2, "q") From 3b2707fff662a1808fd3432b1d7a829bdcc95774 Mon Sep 17 00:00:00 2001 From: Allison Date: Fri, 16 Aug 2024 12:16:35 -0700 Subject: [PATCH 23/35] new line --- Bindings/Python/tests/test_moco.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index 2048005af7..a257b21da5 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -438,4 +438,4 @@ def test_expression_based_parameter_goal(self): sol = study.solve() self.assertAlmostEqual(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass"), - total_weight) \ No newline at end of file + total_weight) From 77ee78a79851d6f431969e3e9d695065a1008337 Mon Sep 17 00:00:00 2001 From: Allison Date: Fri, 16 Aug 2024 13:25:47 -0700 Subject: [PATCH 24/35] replace { with [ for python --- Bindings/Python/tests/test_moco.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index a257b21da5..14a8a70968 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -418,17 +418,18 @@ def test_expression_based_parameter_goal(self): mp = study.updProblem() mp.setModel(createDoubleSlidingMassModel()) mp.setTimeBounds(0, 1) - mp.setStateInfo("/slider/position/value", {-5, 5}, 0, {0.2, 0.3}) - mp.setStateInfo("/slider/position/speed", {-20, 20}) - mp.setStateInfo("/slider2/position/value", {-5, 5}, 1, {1.2, 1.3}) - mp.setStateInfo("/slider2/position/speed", {-20, 20}) + mp.setStateInfo("/slider/position/value", [-5, 5], 0, [0.2, 0.3]) + mp.setStateInfo("/slider/position/speed", [-20, 20]) + mp.setStateInfo("/slider2/position/value", [-5, 5], 1, [1.2, 1.3]) + mp.setStateInfo("/slider2/position/speed", [-20, 20]) parameter = mp.addParameter("sphere_mass", "body", "mass", osim.MocoBounds(0, 10)) parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", osim.MocoBounds(0, 10)) total_weight = 7 - mass_goal = mp.addGoal(osim.MocoExpressionBasedParameterGoal()) + mass_goal = osim.MocoExpressionBasedParameterGoal() + mp.addGoal(mass_goal) mass_goal.setExpression(f"(p+q-{total_weight})^2") mass_goal.addParameter(parameter, "p") mass_goal.addParameter(parameter2, "q") From f3d2fae033f003c447686809991a3c8ac34bb29f Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 16 Aug 2024 14:36:07 -0700 Subject: [PATCH 25/35] Trying to fix failing test --- OpenSim/Auxiliary/auxiliaryTestFunctions.h | 28 ++++++++++++---------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/OpenSim/Auxiliary/auxiliaryTestFunctions.h b/OpenSim/Auxiliary/auxiliaryTestFunctions.h index df97fdda6c..a9e8735c90 100644 --- a/OpenSim/Auxiliary/auxiliaryTestFunctions.h +++ b/OpenSim/Auxiliary/auxiliaryTestFunctions.h @@ -261,21 +261,23 @@ OpenSim::Object* randomize(OpenSim::Object* obj) Property& prop = Property::updAs(ap); prop = SimTK::Vec3(abs(rand()), abs(rand()), abs(rand())); } else if (ts == "Vec6" && !isList) { - // Only property that uses a Vec6 is the inertia property - // Might as well select valid inertias for the purpose of testing Property& prop = Property::updAs(ap); - double Ixx = abs(rand()); - double Ixy = 0.01*Ixx; - prop = SimTK::Vec6(Ixx, Ixx, Ixx, Ixy, Ixy, Ixy); + prop = SimTK::Vec6(abs(rand()), abs(rand()), abs(rand()), + abs(rand()), abs(rand()), abs(rand())); } else if (ts == "string") { - string base("ABCXYZ"); - if (isList){ - stringstream val; - val << base << "_" << ap.size(); - ap.appendValue(val.str()); - } - else{ - ap.updValue() = base; + // The expressions in ExpressionBasedBushingForce cannot be truly + // randomized since they must contain specific variable names (e.g., + // "theta_x", "delta_x", etc.). + if (obj->getConcreteClassName() != "ExpressionBasedBushingForce") { + string base("ABCXYZ"); + if (isList){ + stringstream val; + val << base << "_" << ap.size(); + ap.appendValue(val.str()); + } + else{ + ap.updValue() = base; + } } } else if (ts == "double" && isList && ap.getMaxListSize() < 20) { for (int i=0; i< ap.getMaxListSize(); ++i) From 13b132cb0ca4b64892a1845307f673cf1aec3946 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 16 Aug 2024 15:44:59 -0700 Subject: [PATCH 26/35] Correct CHANGELOG.md --- CHANGELOG.md | 7 ------- 1 file changed, 7 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 391dea965a..b4b65cd569 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -80,13 +80,6 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Fixed bugs in `MocoCasOCProblem` and `CasOC::Problem` with incorrect string formatting. (#3828) - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830) - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834) -- The performance of `getStateVariableValue`, `getStateVariableDerivativeValue`, and `getModelingOption` was improved in - the case where provided string is just the name of the value, rather than a path to it (#3782) -- Fixed bugs in `MocoStepTimeAsymmetryGoal::printDescriptionImpl()` where there were missing or incorrect values printed. (#3842) -- Added `ModOpPrescribeCoordinateValues` which can prescribe motion of joints in a model given a table of data. (#3862) -- Fixed bugs in `convertToMocoTrajectory()` and `MocoTrajectory::resampleWithFrequency()` by updating `interpolate()` to - allow extrapolation using the `extrapolate` flag. Combined with the `ignoreNaNs` flag, this prevents NaNs from - occurring in the output. (#3867) v4.5 From 8610ce0aedea59bfd74b816c04c37bbca4128970 Mon Sep 17 00:00:00 2001 From: Allison Date: Fri, 16 Aug 2024 16:10:20 -0700 Subject: [PATCH 27/35] python create parameter before add --- Bindings/Python/tests/test_moco.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index 14a8a70968..4c3e21f34a 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -423,10 +423,12 @@ def test_expression_based_parameter_goal(self): mp.setStateInfo("/slider2/position/value", [-5, 5], 1, [1.2, 1.3]) mp.setStateInfo("/slider2/position/speed", [-20, 20]) - parameter = mp.addParameter("sphere_mass", "body", "mass", + parameter = osim.MocoParameter("sphere_mass", "body", "mass", osim.MocoBounds(0, 10)) - parameter2 = mp.addParameter("sphere2_mass", "body2", "mass", + mp.addParameter(parameter) + parameter2 = osim.MocoParameter("sphere2_mass", "body2", "mass", osim.MocoBounds(0, 10)) + mp.addParameter2(parameter) total_weight = 7 mass_goal = osim.MocoExpressionBasedParameterGoal() mp.addGoal(mass_goal) From 0c3aeadd2d2494609ec7ce94192b304d7f25dea3 Mon Sep 17 00:00:00 2001 From: Allison Date: Fri, 16 Aug 2024 17:06:01 -0700 Subject: [PATCH 28/35] typo --- Bindings/Python/tests/test_moco.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Bindings/Python/tests/test_moco.py b/Bindings/Python/tests/test_moco.py index 4c3e21f34a..f2c65bf9dd 100644 --- a/Bindings/Python/tests/test_moco.py +++ b/Bindings/Python/tests/test_moco.py @@ -428,7 +428,7 @@ def test_expression_based_parameter_goal(self): mp.addParameter(parameter) parameter2 = osim.MocoParameter("sphere2_mass", "body2", "mass", osim.MocoBounds(0, 10)) - mp.addParameter2(parameter) + mp.addParameter(parameter2) total_weight = 7 mass_goal = osim.MocoExpressionBasedParameterGoal() mp.addGoal(mass_goal) From c767b57b4920cddac6486ebea9224bbb0929b392 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 16 Aug 2024 17:35:58 -0700 Subject: [PATCH 29/35] Revert setting inertia property in auxiliaryTestFunctions --- OpenSim/Auxiliary/auxiliaryTestFunctions.h | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/OpenSim/Auxiliary/auxiliaryTestFunctions.h b/OpenSim/Auxiliary/auxiliaryTestFunctions.h index a9e8735c90..7ff73f296b 100644 --- a/OpenSim/Auxiliary/auxiliaryTestFunctions.h +++ b/OpenSim/Auxiliary/auxiliaryTestFunctions.h @@ -261,9 +261,12 @@ OpenSim::Object* randomize(OpenSim::Object* obj) Property& prop = Property::updAs(ap); prop = SimTK::Vec3(abs(rand()), abs(rand()), abs(rand())); } else if (ts == "Vec6" && !isList) { + // Only property that uses a Vec6 is the inertia property + // Might as well select valid inertias for the purpose of testing Property& prop = Property::updAs(ap); - prop = SimTK::Vec6(abs(rand()), abs(rand()), abs(rand()), - abs(rand()), abs(rand()), abs(rand())); + double Ixx = abs(rand()); + double Ixy = 0.01*Ixx; + prop = SimTK::Vec6(Ixx, Ixx, Ixx, Ixy, Ixy, Ixy); } else if (ts == "string") { // The expressions in ExpressionBasedBushingForce cannot be truly // randomized since they must contain specific variable names (e.g., From 313643f64fa7159c106a61e03953c72f36f431a0 Mon Sep 17 00:00:00 2001 From: Allison Date: Mon, 19 Aug 2024 09:33:30 -0700 Subject: [PATCH 30/35] add to moco.i --- Bindings/moco.i | 1 + 1 file changed, 1 insertion(+) diff --git a/Bindings/moco.i b/Bindings/moco.i index 0d66f84c1e..95af4e3216 100644 --- a/Bindings/moco.i +++ b/Bindings/moco.i @@ -24,6 +24,7 @@ namespace OpenSim { %include %include %include +%include %include %include %include From 884a87a48f0fe06701f4f3c21a8e41063676099b Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 19 Aug 2024 09:54:12 -0700 Subject: [PATCH 31/35] Clarify test change for EBCF; randomize Vec6 properties again --- OpenSim/Auxiliary/auxiliaryTestFunctions.h | 18 +++++++----------- 1 file changed, 7 insertions(+), 11 deletions(-) diff --git a/OpenSim/Auxiliary/auxiliaryTestFunctions.h b/OpenSim/Auxiliary/auxiliaryTestFunctions.h index 7ff73f296b..553796d4fb 100644 --- a/OpenSim/Auxiliary/auxiliaryTestFunctions.h +++ b/OpenSim/Auxiliary/auxiliaryTestFunctions.h @@ -261,24 +261,20 @@ OpenSim::Object* randomize(OpenSim::Object* obj) Property& prop = Property::updAs(ap); prop = SimTK::Vec3(abs(rand()), abs(rand()), abs(rand())); } else if (ts == "Vec6" && !isList) { - // Only property that uses a Vec6 is the inertia property - // Might as well select valid inertias for the purpose of testing Property& prop = Property::updAs(ap); - double Ixx = abs(rand()); - double Ixy = 0.01*Ixx; - prop = SimTK::Vec6(Ixx, Ixx, Ixx, Ixy, Ixy, Ixy); + prop = SimTK::Vec6(abs(rand()), abs(rand()), abs(rand()), + abs(rand()), abs(rand()), abs(rand())); } else if (ts == "string") { - // The expressions in ExpressionBasedBushingForce cannot be truly - // randomized since they must contain specific variable names (e.g., - // "theta_x", "delta_x", etc.). + // We cannot use an arbitrary string for ExpressionBasedBushingForce + // properties since they must contain specific variable names (e.g., + // "theta_x", "delta_x", etc.). if (obj->getConcreteClassName() != "ExpressionBasedBushingForce") { string base("ABCXYZ"); - if (isList){ + if (isList) { stringstream val; val << base << "_" << ap.size(); ap.appendValue(val.str()); - } - else{ + } else { ap.updValue() = base; } } From 5c766836c50f79850c30c6b501324c1a48798fe7 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 19 Aug 2024 12:24:33 -0700 Subject: [PATCH 32/35] Remove cache variable from ExpressionBasedBushingForce --- OpenSim/Auxiliary/auxiliaryTestFunctions.h | 7 +++++-- .../Model/ExpressionBasedBushingForce.cpp | 14 +++----------- .../Simulation/Model/ExpressionBasedBushingForce.h | 6 +----- 3 files changed, 9 insertions(+), 18 deletions(-) diff --git a/OpenSim/Auxiliary/auxiliaryTestFunctions.h b/OpenSim/Auxiliary/auxiliaryTestFunctions.h index 553796d4fb..1f244af7fd 100644 --- a/OpenSim/Auxiliary/auxiliaryTestFunctions.h +++ b/OpenSim/Auxiliary/auxiliaryTestFunctions.h @@ -261,9 +261,12 @@ OpenSim::Object* randomize(OpenSim::Object* obj) Property& prop = Property::updAs(ap); prop = SimTK::Vec3(abs(rand()), abs(rand()), abs(rand())); } else if (ts == "Vec6" && !isList) { + // Only property that uses a Vec6 is the inertia property + // Might as well select valid inertias for the purpose of testing Property& prop = Property::updAs(ap); - prop = SimTK::Vec6(abs(rand()), abs(rand()), abs(rand()), - abs(rand()), abs(rand()), abs(rand())); + double Ixx = abs(rand()); + double Ixy = 0.01*Ixx; + prop = SimTK::Vec6(Ixx, Ixx, Ixx, Ixy, Ixy, Ixy); } else if (ts == "string") { // We cannot use an arbitrary string for ExpressionBasedBushingForce // properties since they must contain specific variable names (e.g., diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp index 1f18cdbbe6..19a9fa8cbd 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp @@ -215,13 +215,6 @@ void ExpressionBasedBushingForce::extendFinalizeFromProperties() } } -void ExpressionBasedBushingForce::extendAddToSystem( - SimTK::MultibodySystem& system) const { - Super::extendAddToSystem(system); - this->_bushingForceCV = addCacheVariable("bushing_force", SimTK::Vec6(0), - SimTK::Stage::Velocity); -} - /** Set the expression for the Mx function and create it's lepton program */ void ExpressionBasedBushingForce::setMxExpression(std::string expression) { @@ -279,7 +272,8 @@ void ExpressionBasedBushingForce::setFzExpression(std::string expression) const SimTK::Vec6& ExpressionBasedBushingForce::getBushingForce( const SimTK::State& s) const { - return getCacheVariableValue(s, _bushingForceCV); + // TODO return a cache variable instead. + return calcBushingForce(s); } //============================================================================= @@ -324,9 +318,7 @@ SimTK::Vec6 ExpressionBasedBushingForce:: SimTK::Vec6 ExpressionBasedBushingForce::calcBushingForce( const SimTK::State& s) const { - Vec6 f = calcStiffnessForce(s) + calcDampingForce(s); - setCacheVariableValue(s, _bushingForceCV, f); - return f; + return calcStiffnessForce(s) + calcDampingForce(s); } diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h index 2f79e410aa..c793e0f8d5 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h @@ -221,8 +221,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); std::string getFzExpression() { return get_Fz_expression(); } /** Get the total bushing force. This is the sum of the stiffness and damping - force contributions. Note, computeForce must be evaluated first, - and this is done automatically if the system is realized to Dynamics. */ + force contributions. */ const SimTK::Vec6& getBushingForce(const SimTK::State& state) const; //-------------------------------------------------------------------------- @@ -276,7 +275,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); // Implement ModelComponent interface. //-------------------------------------------------------------------------- void extendFinalizeFromProperties() override; - void extendAddToSystem(SimTK::MultibodySystem& system) const override; void setNull(); void constructProperties(); @@ -290,8 +288,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); // parser programs for efficiently evaluating the expressions Lepton::ExpressionProgram MxProg, MyProg, MzProg, FxProg, FyProg, FzProg; - mutable CacheVariable _bushingForceCV; - //============================================================================== }; // END of class ExpressionBasedBushingForce //============================================================================== From 8d057ae26c90d920bd157372d0208f069eb5a56b Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 19 Aug 2024 13:36:45 -0700 Subject: [PATCH 33/35] Remove getBushingForce --- .../Model/ExpressionBasedBushingForce.cpp | 6 ------ .../Simulation/Model/ExpressionBasedBushingForce.h | 14 +++++--------- 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp index 19a9fa8cbd..e876f8fb19 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.cpp @@ -270,12 +270,6 @@ void ExpressionBasedBushingForce::setFzExpression(std::string expression) FzProg = Lepton::Parser::parse(expression).optimize().createProgram(); } -const SimTK::Vec6& ExpressionBasedBushingForce::getBushingForce( - const SimTK::State& s) const { - // TODO return a cache variable instead. - return calcBushingForce(s); -} - //============================================================================= // COMPUTATION //============================================================================= diff --git a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h index c793e0f8d5..83d75f86c3 100644 --- a/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h +++ b/OpenSim/Simulation/Model/ExpressionBasedBushingForce.h @@ -96,7 +96,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); //============================================================================== // OUTPUTS //============================================================================== - OpenSim_DECLARE_OUTPUT(bushing_force, SimTK::Vec6, getBushingForce, + OpenSim_DECLARE_OUTPUT(bushing_force, SimTK::Vec6, calcBushingForce, SimTK::Stage::Dynamics); //============================================================================== @@ -220,10 +220,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); theta_x, theta_y, theta_z, delta_x, delta_y, delta_z **/ std::string getFzExpression() { return get_Fz_expression(); } - /** Get the total bushing force. This is the sum of the stiffness and damping - force contributions. */ - const SimTK::Vec6& getBushingForce(const SimTK::State& state) const; - //-------------------------------------------------------------------------- // COMPUTATION //-------------------------------------------------------------------------- @@ -237,6 +233,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); force on frame2 from frame1 in the basis of the deflection rate (dqdot).*/ SimTK::Vec6 calcDampingForce(const SimTK::State& state) const; + /** Calculate the total bushing force. This is the sum of the stiffness and + damping force contributions. */ + SimTK::Vec6 calcBushingForce(const SimTK::State& state) const; + //-------------------------------------------------------------------------- // Reporting //-------------------------------------------------------------------------- @@ -279,10 +279,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ExpressionBasedBushingForce, TwoFrameLinker); void setNull(); void constructProperties(); - /** Calculate the total bushing force. This is the sum of the stiffness and - damping force contributions. */ - SimTK::Vec6 calcBushingForce(const SimTK::State& state) const; - SimTK::Mat66 _dampingMatrix{ 0.0 }; // parser programs for efficiently evaluating the expressions From 6b406cf4cf9ffc0487ba6d84494ce5b07d187821 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 19 Aug 2024 16:59:09 -0700 Subject: [PATCH 34/35] Tighten test tolerance in testForces --- OpenSim/Simulation/Test/testForces.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Test/testForces.cpp b/OpenSim/Simulation/Test/testForces.cpp index 51fc9096e2..66e27a4de5 100644 --- a/OpenSim/Simulation/Test/testForces.cpp +++ b/OpenSim/Simulation/Test/testForces.cpp @@ -193,8 +193,8 @@ TEST_CASE("testExpressionBasedCoordinateForce") { double model_force = spring->getForceMagnitude(osim_state); double output_force = spring->getOutputValue(osim_state, "force_magnitude"); - ASSERT_EQUAL(analytical_force, model_force, 1e-5); - ASSERT_EQUAL(analytical_force, output_force, 1e-5); + ASSERT_EQUAL(analytical_force, model_force, 1e-6); + ASSERT_EQUAL(analytical_force, output_force, 1e-6); } // Test copying From 6e1500b5b094dc9d28fe34067f8a20551a505912 Mon Sep 17 00:00:00 2001 From: Ayman Habib Date: Tue, 20 Aug 2024 10:58:01 -0700 Subject: [PATCH 35/35] Merge Opensim 451 branch back to main (#3882) * Update version.h * set RPATH for tropter lib * Metis5 in opensim 4.5.1 (#3874) * Use different metis source * use metis from sourceforge due to crowdstrike install metis works on osx arm64 but problems downstream * build_metis * Build shared library * Fix metis dylib name on osx * redo configure build install commands metis5 * added catch2 to cmake file * convert testConstaints cases to catch2 * moving around includes * Fix for issue # 3783. Bharghava metabolic model will successfully set and retreive the right muscle mass even when the component already exists in the model. * Adding model to for test case in testMocoMetabolics.cpp. Simple hanging muscle model, but with the Bhargava metabolics component. * Reverting changes to sandbox, and adding test case to testMocoMetabolics.cpp. Test case for deserialization of the metabolics component - checks that the stored mass in the metabolics muscle properties is returned correctly. * Added entry for the Bhargava metabolics bug fix # 3783. * Fixed whitespace issues. * Added empty line at end of file. * Add model to resources for `testMocoMetabolics`. * Update version.h * fix name of metis library on linux * Update CHANGELOG.md upgrade metis on unix, osx [ci skip] * Remove unused script and update CHANGELOG.md * Comments from PR --------- Co-authored-by: Ayman Co-authored-by: Allison Co-authored-by: stingjp --------- Co-authored-by: Ayman Co-authored-by: Allison Co-authored-by: stingjp --- CHANGELOG.md | 3 +-- OpenSim/Moco/CMakeLists.txt | 4 +++- Vendors/tropter/cmake/CMakeLists.txt | 10 +++------- dependencies/CMakeLists.txt | 13 +++++++------ dependencies/get.Metis.patch | 13 ------------- 5 files changed, 14 insertions(+), 29 deletions(-) delete mode 100644 dependencies/get.Metis.patch diff --git a/CHANGELOG.md b/CHANGELOG.md index b4b65cd569..9ce9cffc57 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -17,7 +17,6 @@ v4.6 occurring in the output. (#3867) - Added `Output`s to `ExpressionBasedCoordinateForce`, `ExpressionBasedPointToPointForce`, and `ExpressionBasedBushingForce` for accessing force values. (#3872) - v4.5.1 ====== - Added support for list `Socket`s via the macro `OpenSim_DECLARE_LIST_SOCKET`. The macro-generated method @@ -80,8 +79,8 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781). - Fixed bugs in `MocoCasOCProblem` and `CasOC::Problem` with incorrect string formatting. (#3828) - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830) - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834) +- Upgraded the Ipopt dependency Metis to version 5.1.0 on Unix and macOS to enable building on `osx-arm64` (#3874). - v4.5 ==== - Added `AbstractGeometryPath` which is a base class for `GeometryPath` and other path types (#3388). All path-based diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 77219096fd..c3b904ac21 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -175,7 +175,9 @@ target_compile_definitions(osimMoco PRIVATE OpenSimAddInstallRPATHSelf(TARGET osimMoco LOADER) OpenSimAddInstallRPATHSimbody(TARGET osimMoco LOADER FROM "${CMAKE_INSTALL_LIBDIR}") - +if (OPENSIM_WITH_TROPTER) + OpenSimAddInstallRPATHSelf(TARGET tropter LOADER) +endif() install(TARGETS osimMoco EXPORT OpenSimTargets ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} diff --git a/Vendors/tropter/cmake/CMakeLists.txt b/Vendors/tropter/cmake/CMakeLists.txt index d494b33489..355093dcc3 100644 --- a/Vendors/tropter/cmake/CMakeLists.txt +++ b/Vendors/tropter/cmake/CMakeLists.txt @@ -21,9 +21,7 @@ if(TROPTER_COPY_DEPENDENCIES AND APPLE) ${IPOPT_LIBDIR}/libipopt.dylib ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.3.dylib ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.dylib - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.1.3.10.dylib - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.1.dylib - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.dylib + ${IPOPT_LIBDIR}/../../metis/lib/libmetis.dylib DESTINATION ${CMAKE_INSTALL_LIBDIR}) @@ -38,7 +36,7 @@ if(TROPTER_COPY_DEPENDENCIES AND APPLE) install(SCRIPT "${script}") elseif(TROPTER_COPY_DEPENDENCIES AND UNIX) - message(STATUS "Finding dependencies") + message(STATUS "Finding tropter dependencies") message(STATUS "getting dir for ${pkgcfg_lib_IPOPT_gfortran}" ) message(STATUS "ADOLC_DIR dir ${ADOLC_DIR}" ) message(STATUS "ColPack_ROOT_DIR dir ${ColPack_ROOT_DIR}" ) @@ -64,9 +62,7 @@ elseif(TROPTER_COPY_DEPENDENCIES AND UNIX) ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so.3.0.5 ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so.3 ${IPOPT_LIBDIR}/../../mumps/lib/libcoinmumps.so - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.so.1.3.10 - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.so.1 - ${IPOPT_LIBDIR}/../../metis/lib/libcoinmetis.so + ${IPOPT_LIBDIR}/../../metis/lib/libmetis.so ${gfortran} ${quadmath} diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index b96e8d128e..7ecea1ffa0 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -320,14 +320,15 @@ else() # operators like && is not defined." # Patch the scripts that download Metis and MUMPS to use our # Sourceforge mirror. The original links are not reliable. + message(STATUS "Building Metis...") + ExternalProject_Add(metis - GIT_REPOSITORY https://github.com/coin-or-tools/ThirdParty-Metis.git - GIT_TAG releases/1.3.10 - PATCH_COMMAND cd && ./get.Metis - CONFIGURE_COMMAND /configure --prefix= ADD_FFLAGS=-fallow-argument-mismatch ADD_CFLAGS=-Wno-error=implicit-function-declaration - BUILD_COMMAND ${CMAKE_MAKE_PROGRAM} ${BUILD_FLAGS} + # URL http://glaros.dtc.umn.edu/gkhome/fetch/sw/metis/metis-5.1.0.tar.gz + URL https://sourceforge.net/projects/myosin/files/metis-5.1.0.tar.gz/download + CONFIGURE_COMMAND cd && ${CMAKE_MAKE_PROGRAM} shared=1 config "prefix=${CMAKE_INSTALL_PREFIX}/metis" BUILDDIR=bdir + BUILD_COMMAND cd /bdir && ${CMAKE_MAKE_PROGRAM} INSTALL_DIR "${CMAKE_INSTALL_PREFIX}/metis" - INSTALL_COMMAND ${CMAKE_MAKE_PROGRAM} install) + INSTALL_COMMAND cd /bdir && ${CMAKE_MAKE_PROGRAM} install) # GCC 10 generates a warning when compling MUMPS that we must suppress using # -fallow-argument-mismatch. diff --git a/dependencies/get.Metis.patch b/dependencies/get.Metis.patch deleted file mode 100644 index bfcfa5498e..0000000000 --- a/dependencies/get.Metis.patch +++ /dev/null @@ -1,13 +0,0 @@ ---- get.Metis 2012-05-29 03:55:14.000000000 -0700 -+++ get.Metis 2019-11-19 09:55:59.000000000 -0800 -@@ -22,8 +22,8 @@ - echo " " - - rm -f metis-4.0.3.tar.gz --echo "Downloading the source code from glaros.dtc.umn.edu..." --$wgetcmd http://glaros.dtc.umn.edu/gkhome/fetch/sw/metis/OLD/metis-4.0.3.tar.gz -+echo "Downloading the source code..." -+$wgetcmd https://sourceforge.net/projects/myosin/files/ipopt/metis-4.0.3.tar.gz - - rm -rf metis-4.0 -