Add MocoExpressionBasedParameterGoal

Bindings/Python/tests/test_moco.py

@@ -35,6 +35,27 @@ 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()
+    model.initSystem()
+    return model
+
+
 class TestSwigAddtlInterface(unittest.TestCase):
     def test_bounds(self):
         model = osim.Model()
@@ -391,3 +412,33 @@ 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()
+        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 = osim.MocoParameter("sphere_mass", "body", "mass",
+                                    osim.MocoBounds(0, 10))
+        mp.addParameter(parameter)
+        parameter2 = osim.MocoParameter("sphere2_mass", "body2", "mass",
+                                     osim.MocoBounds(0, 10))
+        mp.addParameter(parameter2)
+        total_weight = 7
+        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")
+
+        ms = study.initTropterSolver()
+        ms.set_num_mesh_intervals(25)
+        sol = study.solve()
+
+        self.assertAlmostEqual(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass"),
+                               total_weight)

Bindings/moco.i

@@ -24,6 +24,7 @@ namespace OpenSim {
 %include <OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h>
 %include <OpenSim/Moco/MocoGoal/MocoContactTrackingGoal.h>
 %include <OpenSim/Moco/MocoGoal/MocoContactImpulseTrackingGoal.h>
+%include <OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h>
 %include <OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h>
 %include <OpenSim/Moco/MocoGoal/MocoJointReactionGoal.h>
 %include <OpenSim/Moco/MocoGoal/MocoSumSquaredStateGoal.h>

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`.

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

OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.cpp

@@ -0,0 +1,125 @@
+/* -------------------------------------------------------------------------- *
+ * OpenSim: MocoExpressionBasedParameterGoal.cpp                              *
+ * -------------------------------------------------------------------------- *
+ * 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 <lepton/Exception.h>
+#include <lepton/ParsedExpression.h>
+#include <lepton/Parser.h>
+#include <OpenSim/Simulation/Model/Model.h>
+
+using namespace OpenSim;
+
+void MocoExpressionBasedParameterGoal::constructProperties() {
+    constructProperty_expression("");
+    constructProperty_parameters();
+    constructProperty_variable_names();
+}
+
+void MocoExpressionBasedParameterGoal::initializeOnModelImpl(const Model& model)
+        const {
+    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);
+
+    for (int i = 0; i < getProperty_parameters().size(); i++) {
+        // 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 and element of the property
+        if (dynamic_cast<const Property<double>*>(ap)) {
+            m_data_types.emplace_back(Type_double);
+        } else {
+            if (dynamic_cast<const Property<SimTK::Vec3>*>(ap)) {
+                m_data_types.emplace_back(Type_Vec3);
+                m_indices.emplace_back(get_parameters(i).getPropertyElement());
+            }
+            else if (dynamic_cast<const Property<SimTK::Vec6>*>(ap)) {
+                m_data_types.emplace_back(Type_Vec6);
+                m_indices.emplace_back(get_parameters(i).getPropertyElement());
+            }
+            else {
+                OPENSIM_THROW_FRMOBJ(Exception,
+                    "Data type of specified model property not supported.");
+            }
+        }
+    }
+
+    // test to make sure all variables are there
+    try
+    {
+        std::map<std::string, double> parameterVars;
+        for (int i = 0; i < getProperty_variable_names().size(); ++i) {
+            parameterVars[get_variable_names(i)] = getPropertyValue(i);
+        }
+        m_program.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 {
+    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<const Property<double>*>(propRef.get())->getValue();
+    }
+    int elt = m_indices[i];
+    if (m_data_types[i] == Type_Vec3) {
+        return static_cast<const Property<SimTK::Vec3>*>(propRef.get())
+                                                     ->getValue()[elt];
+    }
+    if (m_data_types[i] == Type_Vec6) {
+        return static_cast<const Property<SimTK::Vec6>*>(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<std::string, double> parameterVars;
+    for (int i = 0; i < getProperty_variable_names().size(); ++i) {
+        parameterVars[get_variable_names(i)] = getPropertyValue(i);
+    }
+    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("        variable {}: {}", get_variable_names(i),
+                 get_parameters(i).getName());
+    }
+}

OpenSim/Moco/MocoGoal/MocoExpressionBasedParameterGoal.h

@@ -0,0 +1,132 @@
+#ifndef OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H
+#define OPENSIM_MOCOEXPRESSIONBASEDPARAMETERGOAL_H
+/* -------------------------------------------------------------------------- *
+ * OpenSim: MocoExpressionBasedParameterGoal.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 "MocoGoal.h"
+#include "OpenSim/Moco/MocoParameter.h"
+#include <lepton/ExpressionProgram.h>
+#include <SimTKcommon/internal/ReferencePtr.h>
+
+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.
+
+# 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
+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 ^.
+
+# Examples
+
+@code
+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<MocoExpressionBasedParameterGoal>();
+double STIFFNESS = 100.0;
+// minimum is when p + q = STIFFNESS
+spring_goal->setExpression(fmt::format("square( p+q-{} )", STIFFNESS));
+spring_goal->addParameter(*spring1_parameter, "p");
+spring_goal->addParameter(*spring2_parameter, "q");
+@endcode
+
+@ingroup mocogoal */
+class OSIMMOCO_API MocoExpressionBasedParameterGoal : public MocoGoal {
+    OpenSim_DECLARE_CONCRETE_OBJECT(MocoExpressionBasedParameterGoal, MocoGoal);
+
+public:
+    MocoExpressionBasedParameterGoal() { constructProperties(); }
+    MocoExpressionBasedParameterGoal(std::string name)
+            : MocoGoal(std::move(name)) {
+        constructProperties();
+    }
+    MocoExpressionBasedParameterGoal(std::string name, double weight)
+            : MocoGoal(std::move(name), weight) {
+        constructProperties();
+    }
+    MocoExpressionBasedParameterGoal(std::string name, double weight,
+            std::string expression) : MocoGoal(std::move(name), weight) {
+        constructProperties();
+        set_expression(std::move(expression));
+    }
+
+    /** Set the mathematical expression to minimize. Variable names should match
+    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));
+    }
+
+    /** 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(const MocoParameter& parameter, std::string variableName) {
+        append_parameters(parameter);
+        append_variable_names(std::move(variableName));
+    }
+
+protected:
+    void initializeOnModelImpl(const Model& model) const override;
+    void calcGoalImpl(
+            const GoalInput& input, SimTK::Vector& cost) const override;
+    bool getSupportsEndpointConstraintImpl() const override { return true; }
+    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.");
+    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 m_program;
+    // stores references to one property per parameter
+    mutable std::vector<SimTK::ReferencePtr<const AbstractProperty>> m_property_refs;
+    enum DataType {
+      Type_double,
+      Type_Vec3,
+      Type_Vec6
+    };
+    mutable std::vector<DataType> m_data_types;
+    mutable std::vector<int> m_indices;
+
+};
+
+} // namespace OpenSim
+
+#endif // OPENSIM_MOCOPARAMETEREXPRESSIONGOAL_H

OpenSim/Moco/MocoParameter.h

@@ -20,8 +20,8 @@
 
 #include "MocoBounds.h"
 
-#include <OpenSim/Common/Property.h>
 #include <OpenSim/Common/Object.h>
+#include <OpenSim/Common/Property.h>
 #include <SimTKcommon/internal/ReferencePtr.h>
 
 namespace OpenSim {
@@ -141,6 +141,9 @@ class OSIMMOCO_API MocoParameter : public Object {
     {   set_property_name(propertyName); }
     void appendComponentPath(const std::string& componentPath)
     {   append_component_paths(componentPath); }
+    int getPropertyElement() const {
+        return get_property_element();
+    }
 
     /** For use by solvers. This performs error checks and caches information
     about the model that is useful during the optimization.

OpenSim/Moco/RegisterTypes_osimMoco.cpp

@@ -33,6 +33,7 @@
 #include "MocoGoal/MocoContactTrackingGoal.h"
 #include "MocoGoal/MocoControlGoal.h"
 #include "MocoGoal/MocoControlTrackingGoal.h"
+#include "MocoGoal/MocoExpressionBasedParameterGoal.h"
 #include "MocoGoal/MocoGoal.h"
 #include "MocoGoal/MocoInitialActivationGoal.h"
 #include "MocoGoal/MocoInitialForceEquilibriumDGFGoal.h"
@@ -81,6 +82,7 @@ OSIMMOCO_API void RegisterTypes_osimMoco() {
         Object::registerType(MocoControlGoal());
         Object::registerType(MocoSumSquaredStateGoal());
         Object::registerType(MocoControlTrackingGoal());
+        Object::registerType(MocoExpressionBasedParameterGoal());
         Object::registerType(MocoInitialActivationGoal());
         Object::registerType(MocoInitialVelocityEquilibriumDGFGoal());
         Object::registerType(MocoInitialForceEquilibriumDGFGoal());