[WIP] Time-varying state bounds

CHANGELOG.md

@@ -1,3 +1,5 @@
+- 2019-10-20: State bounds can be time-varying with MocoCasADiSolver.
+
 - 2019-10-16: Fix a bug in ModOpscaleMaxIsometricForce, where the scale factor
               was not used properly.
 

Moco/Moco/MocoBounds.cpp

@@ -20,6 +20,8 @@
 
 #include "MocoUtilities.h"
 
+#include <OpenSim/Common/Constant.h>
+
 using namespace OpenSim;
 
 MocoBounds::MocoBounds() {
@@ -33,7 +35,7 @@ MocoBounds::MocoBounds(double value) : MocoBounds() {
 }
 
 MocoBounds::MocoBounds(double lower, double upper) : MocoBounds() {
-    OPENSIM_THROW_IF(SimTK::isNaN(lower) || SimTK::isNaN(upper), Exception, 
+    OPENSIM_THROW_IF(SimTK::isNaN(lower) || SimTK::isNaN(upper), Exception,
         "NaN value detected. Please provide a non-NaN values for the bounds.");
     OPENSIM_THROW_IF(lower > upper, Exception,
         format("Expected lower <= upper, but lower=%g and upper=%g.",
@@ -69,3 +71,35 @@ void MocoBounds::printDescription(std::ostream& stream) const {
     }
     stream.flush();
 }
+
+MocoFunctionBounds::MocoFunctionBounds() {
+    constructProperties();
+}
+
+MocoFunctionBounds::MocoFunctionBounds(const Function& lower)
+        : MocoFunctionBounds() {
+    set_lower_bound(lower);
+}
+
+MocoFunctionBounds::MocoFunctionBounds(const Function& lower,
+        const Function& upper) : MocoFunctionBounds(lower) {
+    set_upper_bound(upper);
+}
+
+
+void MocoFunctionBounds::printDescription(std::ostream& stream) const {
+    if (isEquality()) {
+        stream << get_lower_bound().getConcreteClassName();
+    } else {
+        const auto& lower = get_lower_bound().getConcreteClassName();
+        const auto& upper = get_upper_bound().getConcreteClassName();
+        stream << "[" << lower << ", " << upper << "]";
+    }
+    stream.flush();
+}
+
+void MocoFunctionBounds::constructProperties() {
+    constructProperty_lower_bound(Constant(0));
+    constructProperty_upper_bound();
+    constructProperty_equality_with_lower(false);
+}

Moco/Moco/MocoBounds.h

@@ -23,6 +23,7 @@
 #include <OpenSim/Common/Object.h>
 #include <OpenSim/Common/Property.h>
 #include <OpenSim/Common/Array.h>
+#include <OpenSim/Common/Function.h>
 
 namespace OpenSim {
 
@@ -121,6 +122,50 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoFinalBounds, MocoBounds);
     friend MocoVariableInfo;
 };
 
+class OSIMMOCO_API MocoFunctionBounds : public Object {
+    OpenSim_DECLARE_CONCRETE_OBJECT(MocoFunctionBounds, Object);
+public:
+    MocoFunctionBounds();
+    MocoFunctionBounds(const Function& bounds);
+    MocoFunctionBounds(const Function& lower, const Function& upper);
+    /// True if the lower and upper bounds are the same, resulting in an
+    /// equality constraint.
+    bool isEquality() const {
+        return get_equality_with_lower();
+    }
+    /// Returns true if the provided value is within these bounds.
+    bool isWithinBounds(const double& time, const double& value) const {
+        return calcLower(time) <= value && value <= calcUpper(time);
+    }
+    double calcLower(const double& time) const {
+        SimTK::Vector timeVec(1, &time, true);
+        return get_lower_bound().calcValue(timeVec);
+    }
+    double calcUpper(const double& time) const {
+        if (get_equality_with_lower()) {
+            return calcLower(time);
+        } else {
+            OPENSIM_THROW_IF_FRMOBJ(getProperty_upper_bound().empty(),
+                    Exception, "No upper bound provided.");
+            SimTK::Vector timeVec(1, &time, true);
+            return get_upper_bound().calcValue(timeVec);
+        }
+    }
+
+    void printDescription(std::ostream& stream) const;
+
+private:
+    void constructProperties();
+
+    OpenSim_DECLARE_PROPERTY(
+            lower_bound, Function, "Lower bound as a function of time.");
+    OpenSim_DECLARE_OPTIONAL_PROPERTY(
+            upper_bound, Function, "Upper bound as a function of time.");
+    OpenSim_DECLARE_PROPERTY(equality_with_lower, bool,
+            "The variable must be equal to the lower bound; "
+            "upper must be unspecified (default: false).");
+};
+
 } // namespace OpenSim
 
 #endif // MOCO_MOCOBOUNDS_H

Moco/Moco/MocoCasADiSolver/CasOCProblem.h

@@ -44,6 +44,7 @@ struct Bounds {
     double lower = std::numeric_limits<double>::quiet_NaN();
     double upper = std::numeric_limits<double>::quiet_NaN();
     bool isSet() const { return !std::isnan(lower) && !std::isnan(upper); }
+    bool isEquality() const { return isSet() && lower == upper; }
 };
 
 /// This enum is used to categorize a state variable as a generalized
@@ -54,6 +55,7 @@ enum class KinematicLevel { Position, Velocity, Acceleration };
 struct StateInfo {
     std::string name;
     StateType type;
+    bool usingFunctionBounds;
     Bounds bounds;
     Bounds initialBounds;
     Bounds finalBounds;
@@ -177,19 +179,18 @@ class Problem {
     /// internally handles the differential equations for the generalized
     /// coordinates. The state variables must be added in the order Coordinate,
     /// Speed, Auxiliary.
-    // TODO: Create separate addDegreeOfFreedom() and addAuxiliaryState()?
     void addState(std::string name, StateType type, Bounds bounds,
             Bounds initialBounds, Bounds finalBounds) {
         clipEndpointBounds(bounds, initialBounds);
         clipEndpointBounds(bounds, finalBounds);
-        m_stateInfos.push_back({std::move(name), type, std::move(bounds),
+        m_stateInfos.push_back({std::move(name), type, false, std::move(bounds),
                 std::move(initialBounds), std::move(finalBounds)});
-        if (type == StateType::Coordinate)
-            ++m_numCoordinates;
-        else if (type == StateType::Speed)
-            ++m_numSpeeds;
-        else if (type == StateType::Auxiliary)
-            ++m_numAuxiliaryStates;
+        addStateInternal(type);
+    }
+    void addStateWithTimeVaryingBounds(std::string name, StateType type) {
+        m_usingTimeVaryingBounds = true;
+        m_stateInfos.push_back({std::move(name), type, true, {}, {}, {}});
+        addStateInternal(type);
     }
     /// Add an algebraic variable/"state" to the problem.
     void addControl(std::string name, Bounds bounds, Bounds initialBounds,
@@ -303,6 +304,26 @@ class Problem {
     }
 
 public:
+    struct TimeVaryingBounds {
+        casadi::DM lower;
+        casadi::DM upper;
+    };
+    TimeVaryingBounds findTimeVaryingStateBounds(const std::string& name,
+            const casadi::DM& times) const {
+        const auto it = std::find_if(m_stateInfos.begin(), m_stateInfos.end(),
+                [&name](const StateInfo& s) { return s.name == name; });
+
+        OPENSIM_THROW_IF(it == m_stateInfos.end(),
+                OpenSim::Exception, "State does not exist.");
+        OPENSIM_THROW_IF(!it->usingFunctionBounds, OpenSim::Exception,
+                "State is not using function bounds.");
+
+        return findTimeVaryingStateBoundsImpl(name, times);
+    }
+    virtual TimeVaryingBounds findTimeVaryingStateBoundsImpl(
+            const std::string& name, const casadi::DM& times) const {
+        OPENSIM_THROW(OpenSim::Exception, "Not implemented.");
+    }
     /// Kinematic constraint errors should be ordered as so:
     /// - position-level constraints
     /// - first derivative of position-level constraints
@@ -347,6 +368,12 @@ class Problem {
     /// @}
 
 public:
+    /// Are the initial and final times fixed?
+    bool isFixedTime() const {
+        return m_timeInitialBounds.isEquality() &&
+                m_timeFinalBounds.isEquality();
+    }
+
     /// Create an iterate with the variable names populated according to the
     /// variables added to this problem.
     template <typename IterateType = Iterate>
@@ -384,6 +411,11 @@ class Problem {
     void initialize(const std::string& finiteDiffScheme,
             std::shared_ptr<const std::vector<VariablesDM>>
                     pointsForSparsityDetection) const {
+
+        OPENSIM_THROW_IF(m_usingTimeVaryingBounds && !isFixedTime(),
+                OpenSim::Exception,
+                "Cannot use time-varying bounds for non-fixed-time problems.");
+
         auto* mutThis = const_cast<Problem*>(this);
 
         {
@@ -410,8 +442,9 @@ class Problem {
                         pointsForSparsityDetection);
                 if (info.integrand_function) {
                     info.integrand_function->constructFunction(this,
-                            "endpoint_constraint_" + info.name + "_integrand", index,
-                            finiteDiffScheme, pointsForSparsityDetection);
+                            "endpoint_constraint_" + info.name + "_integrand",
+                            index, finiteDiffScheme,
+                            pointsForSparsityDetection);
                 }
                 ++index;
             }
@@ -549,6 +582,16 @@ class Problem {
     const Bounds& getKinematicConstraintBounds() const {
         return m_kinematicConstraintBounds;
     }
+    const double& getInitialTime() const {
+        OPENSIM_THROW_IF(!isFixedTime(), OpenSim::Exception,
+                "Cannot get initial time if it's not fixed.");
+        return m_timeInitialBounds.lower;
+    }
+    const double& getFinalTime() const {
+        OPENSIM_THROW_IF(!isFixedTime(), OpenSim::Exception,
+                "Cannot get final time if it's not fixed.");
+        return m_timeFinalBounds.lower;
+    }
     const Bounds& getTimeInitialBounds() const { return m_timeInitialBounds; }
     const Bounds& getTimeFinalBounds() const { return m_timeFinalBounds; }
     const std::vector<StateInfo>& getStateInfos() const { return m_stateInfos; }
@@ -599,6 +642,14 @@ class Problem {
     /// @}
 
 private:
+    void addStateInternal(StateType type) {
+        if (type == StateType::Coordinate)
+            ++m_numCoordinates;
+        else if (type == StateType::Speed)
+            ++m_numSpeeds;
+        else if (type == StateType::Auxiliary)
+            ++m_numAuxiliaryStates;
+    }
     /// Clip endpoint to be as strict as b.
     void clipEndpointBounds(const Bounds& b, Bounds& endpoint) {
         endpoint.lower = std::max(b.lower, endpoint.lower);
@@ -608,6 +659,7 @@ class Problem {
     Bounds m_timeInitialBounds;
     Bounds m_timeFinalBounds;
     std::vector<StateInfo> m_stateInfos;
+    bool m_usingTimeVaryingBounds = false;
     int m_numCoordinates = 0;
     int m_numSpeeds = 0;
     int m_numAuxiliaryStates = 0;

Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp

@@ -184,15 +184,35 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid,
     setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds());
 
     {
+        casadi::DM timesDM;
+        if (m_problem.isFixedTime()) {
+            casadi::DM initialTime(casadi::Sparsity::dense(1, 1));
+            initialTime(0, 0) = m_problem.getInitialTime();
+            casadi::DM finalTime(casadi::Sparsity::dense(1, 1));
+            finalTime(0, 0) = m_problem.getFinalTime();
+            timesDM = createTimes(initialTime, finalTime);
+        }
         const auto& stateInfos = m_problem.getStateInfos();
         int is = 0;
         for (const auto& info : stateInfos) {
-            setVariableBounds(
-                    states, is, Slice(1, m_numGridPoints - 1), info.bounds);
-            // The "0" grabs the first column (first mesh point).
-            setVariableBounds(states, is, 0, info.initialBounds);
-            // The "-1" grabs the last column (last mesh point).
-            setVariableBounds(states, is, -1, info.finalBounds);
+            if (info.usingFunctionBounds) {
+                const Problem::TimeVaryingBounds bounds =
+                        m_problem.findTimeVaryingStateBounds(
+                                info.name, timesDM);
+                for (int itime = 0; itime < m_numGridPoints; ++itime) {
+                    setVariableBounds(states, is, itime, 
+                            {bounds.lower(itime).scalar(),
+                             bounds.upper(itime).scalar()});
+                }
+
+            } else {
+                setVariableBounds(
+                        states, is, Slice(1, m_numGridPoints - 1), info.bounds);
+                // The "0" grabs the first column (first mesh point).
+                setVariableBounds(states, is, 0, info.initialBounds);
+                // The "-1" grabs the last column (last mesh point).
+                setVariableBounds(states, is, -1, info.finalBounds);
+            }
             ++is;
         }
     }

Moco/Moco/MocoCasADiSolver/CasOCTranscription.h

@@ -86,8 +86,12 @@ class Transcription {
             const TColumn& columnIndices, const Bounds& bounds) {
         if (bounds.isSet()) {
             const auto& lower = bounds.lower;
-            m_lowerBounds[var](rowIndices, columnIndices) = lower;
             const auto& upper = bounds.upper;
+            OPENSIM_THROW_IF(lower > upper, OpenSim::Exception,
+                    OpenSim::format("Expected lower <= upper, but lower=%d and "
+                                    "upper=%d.",
+                            lower, upper));
+            m_lowerBounds[var](rowIndices, columnIndices) = lower;
             m_upperBounds[var](rowIndices, columnIndices) = upper;
         } else {
             const auto inf = std::numeric_limits<double>::infinity();

Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp

@@ -55,16 +55,20 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver,
             stateType = CasOC::StateType::Speed;
         else
             stateType = CasOC::StateType::Auxiliary;
-        addState(stateName, stateType, convertBounds(info.getBounds()),
-                convertBounds(info.getInitialBounds()),
-                convertBounds(info.getFinalBounds()));
+        if (info.getUseFunctionBounds()) {
+            addStateWithTimeVaryingBounds(stateName, stateType);
+        } else {
+            addState(stateName, stateType, convertBounds(info.getPhaseBounds()),
+                    convertBounds(info.getInitialBounds()),
+                    convertBounds(info.getFinalBounds()));
+        }
     }
 
     auto controlNames =
             createControlNamesFromModel(model, m_modelControlIndices);
     for (const auto& controlName : controlNames) {
         const auto& info = problemRep.getControlInfo(controlName);
-        addControl(controlName, convertBounds(info.getBounds()),
+        addControl(controlName, convertBounds(info.getPhaseBounds()),
                 convertBounds(info.getInitialBounds()),
                 convertBounds(info.getFinalBounds()));
     }
@@ -169,7 +173,7 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver,
                     }
 
                     addKinematicConstraint(multInfo.getName(),
-                            convertBounds(multInfo.getBounds()),
+                            convertBounds(multInfo.getPhaseBounds()),
                             convertBounds(multInfo.getInitialBounds()),
                             convertBounds(multInfo.getFinalBounds()), kinLevel);
 

Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h

@@ -200,6 +200,20 @@ class MocoCasOCProblem : public CasOC::Problem {
     int getJarSize() const { return (int)m_jar->size(); }
 
 private:
+    TimeVaryingBounds findTimeVaryingStateBoundsImpl(
+            const std::string& name, const casadi::DM& times) const override {
+        auto mocoProblemRep = m_jar->take();
+        const auto& bounds =
+                mocoProblemRep->getStateInfo(name).getFunctionBounds();
+        casadi::DM L(casadi::Sparsity::dense(times.rows(), times.columns()));
+        casadi::DM U(casadi::Sparsity::dense(times.rows(), times.columns()));
+        for (int i = 0; i < times.numel(); ++i) {
+            L(i) = bounds.calcLower(times(i).scalar());
+            U(i) = bounds.calcUpper(times(i).scalar());
+        }
+        m_jar->leave(std::move(mocoProblemRep));
+        return {std::move(L), std::move(U)};
+    }
     void calcMultibodySystemExplicit(const ContinuousInput& input,
             bool calcKCErrors,
             MultibodySystemExplicitOutput& output) const override {