From 7f3d3861dfce99f36d56426b367e79bf767e59c1 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Mon, 14 Oct 2019 09:00:29 -0700 Subject: [PATCH 1/8] WIP: Bounds can be functions of time. --- CHANGELOG.md | 10 +++++++ Moco/Moco/MocoBounds.h | 52 ++++++++++++++++++++++++++++++++++ Moco/Moco/MocoTrack.cpp | 9 ++++++ Moco/Moco/MocoUtilities.cpp | 7 +++++ Moco/Moco/MocoUtilities.h | 5 ++++ Moco/Moco/MocoVariableInfo.cpp | 11 +++++++ Moco/Moco/MocoVariableInfo.h | 2 ++ 7 files changed, 96 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 5ef91c876..e349f2ecf 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,3 +1,13 @@ +- 2019-10-13: State and control bounds can be time-varying with + MocoCasADiSolver. + + TODO: + - MocoTropterSolver must throw exception. + - printDescription(). + - Only works for fixed-time problems. + - Must be able to evaluate the function across the entire time + range. + - 2019-10-04: report.py can plot normalized tendon force states. Users can provide a MocoStudy file instead of a Model file, and users can specify the name of the report output file. diff --git a/Moco/Moco/MocoBounds.h b/Moco/Moco/MocoBounds.h index 756b368e0..1b88fb3bd 100644 --- a/Moco/Moco/MocoBounds.h +++ b/Moco/Moco/MocoBounds.h @@ -23,6 +23,7 @@ #include #include #include +#include namespace OpenSim { @@ -121,6 +122,57 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoFinalBounds, MocoBounds); friend MocoVariableInfo; }; +class OSIMMOCO_API MocoFunctionBounds : public Object { + OpenSim_DECLARE_CONCRETE_OBJECT(MocoFunctionBounds, Object); +public: + MocoFunctionBounds(); + MocoFunctionBounds(Function bounds); + MocoFunctionBounds(Function lower, Function upper); + /// Create bounds that are (-inf, inf), so the variable is unconstrained. + static MocoBounds unconstrained() { + return MocoBounds(-SimTK::Infinity, SimTK::Infinity); + } + /// True if the lower and upper bounds are both not NaN. + bool isSet() const { + return !getProperty_bounds().empty(); + } + /// True if the lower and upper bounds are the same, resulting in an + /// equality constraint. + bool isEquality() const { + return isSet() && getLower() == getUpper(); + } + /// Returns true if the provided value is within these bounds. + bool isWithinBounds(const double& value) const { + return getLower() <= value && value <= getUpper(); + } + double getLower() const { + if (!isSet()) { + return SimTK::NTraits::getNaN(); + } else { + return get_bounds(0); + } + } + double getUpper() const { + if (!isSet()) { + return SimTK::NTraits::getNaN(); + } else if (getProperty_bounds().size() == 1) { + return get_bounds(0); + } else { + return get_bounds(1); + } + } + + void printDescription(std::ostream& stream) const; +private: + OpenSim_DECLARE_OPTIONAL_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 diff --git a/Moco/Moco/MocoTrack.cpp b/Moco/Moco/MocoTrack.cpp index ad82a80e5..a8bf369df 100644 --- a/Moco/Moco/MocoTrack.cpp +++ b/Moco/Moco/MocoTrack.cpp @@ -102,6 +102,15 @@ MocoStudy MocoTrack::initialize() { } problem.setTimeBounds(m_timeInfo.initial, m_timeInfo.final); + // Set state bounds. + // ----------------- + if (get_tight_state_bounds() /*TODO Rename*/) { + setKinematicsFunctionBoundsFromTable(problem, tracked_states, + get_state_bound_range_rotational(), + get_state_bound_range_translational()); + } + + // Configure solver. // ----------------- MocoCasADiSolver& solver = study.initCasADiSolver(); diff --git a/Moco/Moco/MocoUtilities.cpp b/Moco/Moco/MocoUtilities.cpp index 0edd330d3..30d7f8593 100644 --- a/Moco/Moco/MocoUtilities.cpp +++ b/Moco/Moco/MocoUtilities.cpp @@ -771,3 +771,10 @@ TimeSeriesTable OpenSim::createExternalLoadsTableForGait(Model model, return externalForcesTableFlat; } + +void OpenSim::setKinematicsFunctionBoundsFromTable(MocoProblem& problem, + const TimeSeriesTable& kinematics, const double& rangeRotational, + const double& rangeTranslational) { + // TODO: get the states from the model. + // TOOD: create GCVSplineSet? how to offset? +} diff --git a/Moco/Moco/MocoUtilities.h b/Moco/Moco/MocoUtilities.h index e51a2a4d7..24b200ed4 100644 --- a/Moco/Moco/MocoUtilities.h +++ b/Moco/Moco/MocoUtilities.h @@ -772,6 +772,11 @@ TimeSeriesTable createExternalLoadsTableForGait(Model model, const std::vector& forceNamesRightFoot, const std::vector& forceNamesLeftFoot); +OSIMMOCO_API +void setKinematicsFunctionBoundsFromTable(MocoProblem& problem, + const TimeSeriesTable& kinematics, const double& rangeRotational, + const double& rangeTranslational); + } // namespace OpenSim #endif // MOCO_MOCOUTILITIES_H diff --git a/Moco/Moco/MocoVariableInfo.cpp b/Moco/Moco/MocoVariableInfo.cpp index e6b05be9e..21bdae485 100644 --- a/Moco/Moco/MocoVariableInfo.cpp +++ b/Moco/Moco/MocoVariableInfo.cpp @@ -35,6 +35,17 @@ MocoVariableInfo::MocoVariableInfo(const std::string& name, validate(); } +MocoVariableInfo::MocoVariableInfo(const std::string& name, + const MocoFunctionBounds&, + const MocoInitialBounds&, const MocoFinalBounds&) + : MocoVariableInfo() { + setName(name); + set_bounds(bounds.getAsArray()); + set_initial_bounds(initial.getAsArray()); + set_final_bounds(final.getAsArray()); + // TODO validate(); +} + void MocoVariableInfo::validate() const { const auto& n = getName(); const auto b = getBounds(); diff --git a/Moco/Moco/MocoVariableInfo.h b/Moco/Moco/MocoVariableInfo.h index 0774071d6..3b1fac3bc 100644 --- a/Moco/Moco/MocoVariableInfo.h +++ b/Moco/Moco/MocoVariableInfo.h @@ -35,6 +35,8 @@ class OSIMMOCO_API MocoVariableInfo : public Object { MocoVariableInfo(); MocoVariableInfo(const std::string& name, const MocoBounds&, const MocoInitialBounds&, const MocoFinalBounds&); + MocoVariableInfo(const std::string& name, const MocoFunctionBounds&, + const MocoInitialBounds&, const MocoFinalBounds&); /// @details Note: the return value is constructed fresh on every call from /// the internal property. Avoid repeated calls to this function. From 5e48481ce8afde197e33a363209eb006d4ab8cfc Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Tue, 15 Oct 2019 00:29:22 -0700 Subject: [PATCH 2/8] Propagating changes for time-varying bounds. --- CHANGELOG.md | 1 + Moco/Moco/MocoBounds.cpp | 36 +++++++++ Moco/Moco/MocoBounds.h | 45 +++++------- Moco/Moco/MocoCasADiSolver/CasOCProblem.h | 64 +++++++++++++--- .../MocoCasADiSolver/CasOCTranscription.cpp | 26 +++++-- .../MocoCasADiSolver/MocoCasOCProblem.cpp | 10 ++- Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h | 12 +++ Moco/Moco/MocoProblem.cpp | 14 ++++ Moco/Moco/MocoProblem.h | 4 + Moco/Moco/MocoTrack.cpp | 17 ++++- Moco/Moco/MocoUtilities.cpp | 53 +++++++++++++- Moco/Moco/MocoUtilities.h | 5 +- Moco/Moco/MocoVariableInfo.cpp | 73 ++++++++++--------- Moco/Moco/MocoVariableInfo.h | 32 +++++--- 14 files changed, 293 insertions(+), 99 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e349f2ecf..633f34277 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -7,6 +7,7 @@ - Only works for fixed-time problems. - Must be able to evaluate the function across the entire time range. + - validate that lower <= upper across the time range. - 2019-10-04: report.py can plot normalized tendon force states. Users can provide a MocoStudy file instead of a Model file, and users can diff --git a/Moco/Moco/MocoBounds.cpp b/Moco/Moco/MocoBounds.cpp index 297d90fa2..a67e0cef2 100644 --- a/Moco/Moco/MocoBounds.cpp +++ b/Moco/Moco/MocoBounds.cpp @@ -20,6 +20,8 @@ #include "MocoUtilities.h" +#include + using namespace OpenSim; MocoBounds::MocoBounds() { @@ -69,3 +71,37 @@ 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 { + // TODO: Print min and max of the functions. + 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); +} diff --git a/Moco/Moco/MocoBounds.h b/Moco/Moco/MocoBounds.h index 1b88fb3bd..cdfb98491 100644 --- a/Moco/Moco/MocoBounds.h +++ b/Moco/Moco/MocoBounds.h @@ -126,45 +126,38 @@ class OSIMMOCO_API MocoFunctionBounds : public Object { OpenSim_DECLARE_CONCRETE_OBJECT(MocoFunctionBounds, Object); public: MocoFunctionBounds(); - MocoFunctionBounds(Function bounds); - MocoFunctionBounds(Function lower, Function upper); - /// Create bounds that are (-inf, inf), so the variable is unconstrained. - static MocoBounds unconstrained() { - return MocoBounds(-SimTK::Infinity, SimTK::Infinity); - } - /// True if the lower and upper bounds are both not NaN. - bool isSet() const { - return !getProperty_bounds().empty(); - } + 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 isSet() && getLower() == getUpper(); + return get_equality_with_lower(); } /// Returns true if the provided value is within these bounds. - bool isWithinBounds(const double& value) const { - return getLower() <= value && value <= getUpper(); + bool isWithinBounds(const double& time, const double& value) const { + return findLower(time) <= value && value <= findUpper(time); } - double getLower() const { - if (!isSet()) { - return SimTK::NTraits::getNaN(); - } else { - return get_bounds(0); - } + double findLower(const double& time) const { + SimTK::Vector timeVec(1, &time, true); + return get_lower_bound().calcValue(timeVec); } - double getUpper() const { - if (!isSet()) { - return SimTK::NTraits::getNaN(); - } else if (getProperty_bounds().size() == 1) { - return get_bounds(0); + double findUpper(const double& time) const { + if (get_equality_with_lower()) { + return findLower(time); } else { - return get_bounds(1); + 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: - OpenSim_DECLARE_OPTIONAL_PROPERTY( + 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."); diff --git a/Moco/Moco/MocoCasADiSolver/CasOCProblem.h b/Moco/Moco/MocoCasADiSolver/CasOCProblem.h index 688ede15e..3dac83b39 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/Moco/Moco/MocoCasADiSolver/CasOCProblem.h @@ -44,6 +44,7 @@ struct Bounds { double lower = std::numeric_limits::quiet_NaN(); double upper = std::numeric_limits::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,20 @@ class Problem { } public: + casadi::DM findTimeVaryingStateLowerBounds(const std::string& name, + const casadi::DM& times) const { + checkStateNameForTimeVaryingBounds(name); + return findTimeVaryingStateLowerBoundsImpl(name, times); + } + casadi::DM findTimeVaryingStateUpperBounds(const std::string& name, + const casadi::DM& times) const { + checkStateNameForTimeVaryingBounds(name); + return findTimeVaryingStateUpperBoundsImpl(name, times); + } + virtual casadi::DM findTimeVaryingStateLowerBoundsImpl( + const std::string& name, const casadi::DM& times) const = 0; + virtual casadi::DM findTimeVaryingStateUpperBoundsImpl( + const std::string& name, const casadi::DM& times) const = 0; /// Kinematic constraint errors should be ordered as so: /// - position-level constraints /// - first derivative of position-level constraints @@ -347,6 +362,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 @@ -384,6 +405,10 @@ class Problem { void initialize(const std::string& finiteDiffScheme, std::shared_ptr> pointsForSparsityDetection) const { + OPENSIM_THROW_IF(m_usingTimeVaryingBounds && !isFixedTime(), + OpenSim::Exception, + "Cannot use time-varying bounds for non-fixed-time problems."); + auto* mutThis = const_cast(this); { @@ -410,8 +435,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; } @@ -599,15 +625,33 @@ 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); endpoint.upper = std::min(b.upper, endpoint.upper); } + void checkStateNameForTimeVaryingBounds(const std::string& name) 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."); + } Bounds m_timeInitialBounds; Bounds m_timeFinalBounds; std::vector m_stateInfos; + bool m_usingTimeVaryingBounds = false; int m_numCoordinates = 0; int m_numSpeeds = 0; int m_numAuxiliaryStates = 0; diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp index b8248ef44..0753d0fe9 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -187,12 +187,26 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, 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 casadi::DM lower = + m_problem.findTimeVaryingStateLowerBounds(info.name, + m_times); + const casadi::DM upper = + m_problem.findTimeVaryingStateUpperBounds(info.name, + m_times); + for (int itime = 0; itime < m_numGridPoints; ++itime) { + setVariableBounds(states, is, itime, + {lower(itime, 0), upper(itime, 0)}); + } + + } 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; } } diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 891a25129..5620217ea 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -55,9 +55,13 @@ 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.getBounds()), + convertBounds(info.getInitialBounds()), + convertBounds(info.getFinalBounds())); + } } auto controlNames = diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 2470b42fd..95663cda1 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -200,6 +200,18 @@ class MocoCasOCProblem : public CasOC::Problem { int getJarSize() const { return (int)m_jar->size(); } private: + casadi::DM findTimeVaryingStateLowerBoundsImpl( + const std::string& name, const casadi::DM& times) const override { + auto mocoProblemRep = m_jar->take(); + for (int i = 0; i < times.numel(); ++i) { + mocoProblemRep->getState + } + + } + casadi::DM findTimeVaryingStateUpperBoundsImpl( + const std::string& name, const casadi::DM& times) const override { + + }; void calcMultibodySystemExplicit(const ContinuousInput& input, bool calcKCErrors, MultibodySystemExplicitOutput& output) const override { diff --git a/Moco/Moco/MocoProblem.cpp b/Moco/Moco/MocoProblem.cpp index 1e3d8b329..6815cde2d 100644 --- a/Moco/Moco/MocoProblem.cpp +++ b/Moco/Moco/MocoProblem.cpp @@ -93,6 +93,16 @@ void MocoPhase::setStateInfo(const std::string& name, const MocoBounds& bounds, else upd_state_infos(idx) = info; } +void MocoPhase::setStateInfo( + const std::string& name, const MocoFunctionBounds& fb) { + int idx = getProperty_state_infos().findIndexForName(name); + + MocoVariableInfo info(name, fb); + if (idx == -1) + append_state_infos(info); + else + upd_state_infos(idx) = info; +} void MocoPhase::setStateInfoPattern(const std::string& pattern, const MocoBounds& bounds, const MocoInitialBounds& initial, const MocoFinalBounds& final) { @@ -238,6 +248,10 @@ void MocoProblem::setStateInfo(const std::string& name, const MocoFinalBounds& final) { upd_phases(0).setStateInfo(name, bounds, initial, final); } +void MocoProblem::setStateInfo(const std::string& name, + const MocoFunctionBounds& fb) { + upd_phases(0).setStateInfo(name, fb); +} void MocoProblem::printControlNamesWithSubstring(const std::string& name) { upd_phases(0).printControlNamesWithSubstring(name); } diff --git a/Moco/Moco/MocoProblem.h b/Moco/Moco/MocoProblem.h index 3c7845121..c9a04c720 100644 --- a/Moco/Moco/MocoProblem.h +++ b/Moco/Moco/MocoProblem.h @@ -137,6 +137,8 @@ class OSIMMOCO_API MocoPhase : public Object { void setStateInfo(const std::string& name, const MocoBounds& bounds, const MocoInitialBounds& init = {}, const MocoFinalBounds& final = {}); + /// TODO + void setStateInfo(const std::string& name, const MocoFunctionBounds& fb); /// Set information for state variables whose names match the provided /// regular expression. You can use this to set bounds for all muscle /// activations, etc. Infos provided via setStateInfoPattern() take @@ -450,6 +452,8 @@ class OSIMMOCO_API MocoProblem : public Object { /// Set bounds for a state variable for phase 0. void setStateInfo(const std::string& name, const MocoBounds&, const MocoInitialBounds& = {}, const MocoFinalBounds& = {}); + /// TODO setStateInfoWithFunction()? + void setStateInfo(const std::string& name, const MocoFunctionBounds& fb); /// Set bounds for all state variables for phase 0 whose path matches /// the provided pattern. // TODO: We tried to give an example regex but it had characters that caused diff --git a/Moco/Moco/MocoTrack.cpp b/Moco/Moco/MocoTrack.cpp index a8bf369df..a801ce935 100644 --- a/Moco/Moco/MocoTrack.cpp +++ b/Moco/Moco/MocoTrack.cpp @@ -104,10 +104,19 @@ MocoStudy MocoTrack::initialize() { // Set state bounds. // ----------------- - if (get_tight_state_bounds() /*TODO Rename*/) { - setKinematicsFunctionBoundsFromTable(problem, tracked_states, - get_state_bound_range_rotational(), - get_state_bound_range_translational()); + // TODO: Default true. Make it an optional property. + if (get_bound_kinematic_states_with_states_reference()) { + OPENSIM_THROW_IF_FRMOBJ(get_states_reference().empty(), + Exception, + "Cannot bound kinematic states with states reference if no " + "states reference is supplied."); + setKinematicStateFunctionBoundsFromTable(model, + tracked_states, + // TODO: use properties for these ranges. + SimTK::convertDegreesToRadians(10), 0.10, + problem); + // get_state_bound_range_rotational(), + // get_state_bound_range_translational()); } diff --git a/Moco/Moco/MocoUtilities.cpp b/Moco/Moco/MocoUtilities.cpp index 30d7f8593..6fd43c9cf 100644 --- a/Moco/Moco/MocoUtilities.cpp +++ b/Moco/Moco/MocoUtilities.cpp @@ -772,9 +772,54 @@ TimeSeriesTable OpenSim::createExternalLoadsTableForGait(Model model, return externalForcesTableFlat; } -void OpenSim::setKinematicsFunctionBoundsFromTable(MocoProblem& problem, +void OpenSim::setKinematicStateFunctionBoundsFromTable(const Model& model, const TimeSeriesTable& kinematics, const double& rangeRotational, - const double& rangeTranslational) { - // TODO: get the states from the model. - // TOOD: create GCVSplineSet? how to offset? + const double& rangeTranslational, + MocoProblem& problem) { + + OPENSIM_THROW_IF(rangeRotational < 0, Exception, + format("Expected rangeRotational to be non-negative, but got {}.", + rangeRotational)); + OPENSIM_THROW_IF(rangeTranslational < 0, Exception, + format("Expected rangeTranslational to be non-negative, " + "but got {}.", rangeTranslational)); + + const auto coords = model.getCoordinatesInMultibodyTreeOrder(); + const int numRows = (int)kinematics.getNumRows(); + const auto& time = kinematics.getIndependentColumn(); + const std::unordered_map ranges = + {{Coordinate::Rotational, rangeRotational}, + {Coordinate::Translational, rangeTranslational}}; + for (const auto& coord : coords) { + const auto& motionType = coord->getMotionType(); + // We can't set the bounds for coupled coordinates in a generic way, + // so we ignore. + if (motionType == Coordinate::Rotational || + motionType == Coordinate::Translational) { + const auto& valueStr = coord->getStateVariableNames().get(0); + if (kinematics.hasColumn(valueStr)) { + const auto& column = kinematics.getDependentColumn(valueStr); + SimTK::Vector temp = column - ranges[motionType]; + GCVSpline lower(5, numRows, time.data(), + temp.getContiguousScalarData()); + temp = column + ranges[motionType]; + GCVSpline upper(5, numRows, time.data(), + temp.getContiguousScalarData()); + problem.setStateInfo(valueStr, + {std::move(lower), std::move(upper)}); + } + const auto& speedStr = coord->getStateVariableNames().get(1); + if (kinematics.hasColumn(speedStr)) { + const auto& column = kinematics.getDependentColumn(speedStr); + SimTK::Vector temp = column - ranges[motionType]; + GCVSpline lower(5, numRows, time.data(), + temp.getContiguousScalarData()); + temp = column + ranges[motionType]; + GCVSpline upper(5, numRows, time.data(), + temp.getContiguousScalarData()); + problem.setStateInfo(speedStr, + {std::move(lower), std::move(upper)}); + } + } + } } diff --git a/Moco/Moco/MocoUtilities.h b/Moco/Moco/MocoUtilities.h index 24b200ed4..6029f5ebd 100644 --- a/Moco/Moco/MocoUtilities.h +++ b/Moco/Moco/MocoUtilities.h @@ -772,10 +772,11 @@ TimeSeriesTable createExternalLoadsTableForGait(Model model, const std::vector& forceNamesRightFoot, const std::vector& forceNamesLeftFoot); +/// TODO OSIMMOCO_API -void setKinematicsFunctionBoundsFromTable(MocoProblem& problem, +void setKinematicStateFunctionBoundsFromTable(const Model& model, const TimeSeriesTable& kinematics, const double& rangeRotational, - const double& rangeTranslational); + const double& rangeTranslational, MocoProblem& problem); } // namespace OpenSim diff --git a/Moco/Moco/MocoVariableInfo.cpp b/Moco/Moco/MocoVariableInfo.cpp index 21bdae485..c9bcddf73 100644 --- a/Moco/Moco/MocoVariableInfo.cpp +++ b/Moco/Moco/MocoVariableInfo.cpp @@ -17,82 +17,85 @@ * -------------------------------------------------------------------------- */ #include "MocoVariableInfo.h" + #include "MocoUtilities.h" using namespace OpenSim; -MocoVariableInfo::MocoVariableInfo() { - constructProperties(); -} +MocoVariableInfo::MocoVariableInfo() { constructProperties(); } MocoVariableInfo::MocoVariableInfo(const std::string& name, const MocoBounds& bounds, const MocoInitialBounds& initial, - const MocoFinalBounds& final) : MocoVariableInfo() { + const MocoFinalBounds& final) + : MocoVariableInfo() { setName(name); - set_bounds(bounds.getAsArray()); + set_phase_bounds(bounds.getAsArray()); set_initial_bounds(initial.getAsArray()); set_final_bounds(final.getAsArray()); validate(); } -MocoVariableInfo::MocoVariableInfo(const std::string& name, - const MocoFunctionBounds&, - const MocoInitialBounds&, const MocoFinalBounds&) +MocoVariableInfo::MocoVariableInfo( + const std::string& name, const MocoFunctionBounds& bounds) : MocoVariableInfo() { setName(name); - set_bounds(bounds.getAsArray()); - set_initial_bounds(initial.getAsArray()); - set_final_bounds(final.getAsArray()); - // TODO validate(); + set_function_bounds(bounds); + set_use_function_bounds(true); } void MocoVariableInfo::validate() const { const auto& n = getName(); - const auto b = getBounds(); + const auto b = getPhaseBounds(); const auto ib = getInitialBounds(); const auto fb = getFinalBounds(); OPENSIM_THROW_IF(ib.isSet() && ib.getLower() < b.getLower(), Exception, format("For variable %s, expected " - "[initial value lower bound] >= [lower bound], but " - "intial value lower bound=%g, lower bound=%g.", + "[initial value lower bound] >= [phase lower bound], but " + "intial value lower bound=%g, phase lower bound=%g.", n, ib.getLower(), b.getLower())); OPENSIM_THROW_IF(fb.isSet() && fb.getLower() < b.getLower(), Exception, format("For variable %s, expected " - "[final value lower bound] >= [lower bound], but " - "final value lower bound=%g, lower bound=%g.", + "[final value lower bound] >= [phase lower bound], but " + "final value lower bound=%g, phase lower bound=%g.", n, fb.getLower(), b.getLower())); OPENSIM_THROW_IF(ib.isSet() && ib.getUpper() > b.getUpper(), Exception, format("For variable %s, expected " - "[initial value upper bound] >= [upper bound], but " - "initial value upper bound=%g, upper bound=%g.", + "[initial value upper bound] >= [phase upper bound], but " + "initial value upper bound=%g, phase upper bound=%g.", n, ib.getUpper(), b.getUpper())); OPENSIM_THROW_IF(fb.isSet() && fb.getUpper() > b.getUpper(), Exception, format("For variable %s, expected " - "[final value upper bound] >= [upper bound], but " - "final value upper bound=%g, upper bound=%g.", + "[final value upper bound] >= [phase upper bound], but " + "final value upper bound=%g, phase upper bound=%g.", n, fb.getUpper(), b.getUpper())); } void MocoVariableInfo::printDescription(std::ostream& stream) const { - const auto bounds = getBounds(); - stream << getName() << ". bounds: "; - bounds.printDescription(stream); - const auto initial = getInitialBounds(); - if (initial.isSet()) { - stream << " initial: "; - initial.printDescription(stream); - } - const auto final = getFinalBounds(); - if (final.isSet()) { - stream << " final: "; - final.printDescription(stream); + if (get_use_function_bounds()) { + get_function_bounds().printDescription(stream); + } else { + const auto bounds = getPhaseBounds(); + stream << getName() << ". bounds: "; + bounds.printDescription(stream); + const auto initial = getInitialBounds(); + if (initial.isSet()) { + stream << " initial: "; + initial.printDescription(stream); + } + const auto final = getFinalBounds(); + if (final.isSet()) { + stream << " final: "; + final.printDescription(stream); + } } stream << std::endl; } void MocoVariableInfo::constructProperties() { - constructProperty_bounds(); + constructProperty_phase_bounds(); constructProperty_initial_bounds(); constructProperty_final_bounds(); -} \ No newline at end of file + constructProperty_function_bounds(); + constructProperty_use_function_bounds(false); +} diff --git a/Moco/Moco/MocoVariableInfo.h b/Moco/Moco/MocoVariableInfo.h index 3b1fac3bc..bf0cc2344 100644 --- a/Moco/Moco/MocoVariableInfo.h +++ b/Moco/Moco/MocoVariableInfo.h @@ -35,23 +35,33 @@ class OSIMMOCO_API MocoVariableInfo : public Object { MocoVariableInfo(); MocoVariableInfo(const std::string& name, const MocoBounds&, const MocoInitialBounds&, const MocoFinalBounds&); - MocoVariableInfo(const std::string& name, const MocoFunctionBounds&, - const MocoInitialBounds&, const MocoFinalBounds&); + MocoVariableInfo(const std::string& name, const MocoFunctionBounds&); + + /// TODO + bool getUseFunctionBounds() const { return get_use_function_bounds(); } /// @details Note: the return value is constructed fresh on every call from /// the internal property. Avoid repeated calls to this function. - MocoBounds getBounds() const { return MocoBounds(getProperty_bounds()); } - /// @copydoc getBounds() + MocoBounds getPhaseBounds() const { + OPENSIM_THROW_IF_FRMOBJ(getUseFunctionBounds(), Exception, + "This info is set to use function bounds."); + return MocoBounds(getProperty_phase_bounds()); + } + /// @copydoc getPhaseBounds() MocoInitialBounds getInitialBounds() const { + OPENSIM_THROW_IF_FRMOBJ(getUseFunctionBounds(), Exception, + "This info is set to use function bounds."); return MocoInitialBounds(getProperty_initial_bounds()); } - /// @copydoc getBounds() + /// @copydoc getPhaseBounds() MocoFinalBounds getFinalBounds() const { + OPENSIM_THROW_IF_FRMOBJ(getUseFunctionBounds(), Exception, + "This info is set to use function bounds."); return MocoFinalBounds(getProperty_final_bounds()); } - void setBounds(const MocoBounds& bounds) { - set_bounds(bounds.getAsArray()); + void setPhaseBounds(const MocoBounds& bounds) { + set_phase_bounds(bounds.getAsArray()); } void setInitialBounds(const MocoInitialBounds& bounds) { set_initial_bounds(bounds.getAsArray()); @@ -60,7 +70,8 @@ class OSIMMOCO_API MocoVariableInfo : public Object { set_final_bounds(bounds.getAsArray()); } - /// Throws an exception if initial and final bounds are not within bounds. + /// Throws an exception if initial and final bounds are not within phase + /// bounds. This does not check the function bounds. // TODO Move to finalizeFromProperties() and cache MocoBounds. void validate() const; @@ -68,7 +79,7 @@ class OSIMMOCO_API MocoVariableInfo : public Object { void printDescription(std::ostream& stream = std::cout) const; protected: - OpenSim_DECLARE_LIST_PROPERTY_ATMOST(bounds, double, 2, + OpenSim_DECLARE_LIST_PROPERTY_ATMOST(phase_bounds, double, 2, "1 value: required value over all time. " "2 values: lower, upper bounds on value over all time."); OpenSim_DECLARE_LIST_PROPERTY_ATMOST(initial_bounds, double, 2, @@ -77,6 +88,9 @@ class OSIMMOCO_API MocoVariableInfo : public Object { OpenSim_DECLARE_LIST_PROPERTY_ATMOST(final_bounds, double, 2, "1 value: required final value. " "2 values: lower, upper bounds on final value."); + OpenSim_DECLARE_OPTIONAL_PROPERTY(function_bounds, MocoFunctionBounds, + "TODO"); + OpenSim_DECLARE_PROPERTY(use_function_bounds, bool, "TODO"); private: void constructProperties(); From db96f82563660196816e50f7ef64ab4bdecb00c2 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Tue, 15 Oct 2019 11:41:52 -0700 Subject: [PATCH 3/8] Propagating changes for time-varying bounds. --- Moco/Moco/MocoBounds.h | 6 +++--- Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h | 14 +++++++++++--- Moco/Moco/MocoVariableInfo.h | 12 ++++++++++++ 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/Moco/Moco/MocoBounds.h b/Moco/Moco/MocoBounds.h index cdfb98491..da7ac42a8 100644 --- a/Moco/Moco/MocoBounds.h +++ b/Moco/Moco/MocoBounds.h @@ -137,13 +137,13 @@ class OSIMMOCO_API MocoFunctionBounds : public Object { bool isWithinBounds(const double& time, const double& value) const { return findLower(time) <= value && value <= findUpper(time); } - double findLower(const double& time) const { + double calcLower(const double& time) const { SimTK::Vector timeVec(1, &time, true); return get_lower_bound().calcValue(timeVec); } - double findUpper(const double& time) const { + double calcUpper(const double& time) const { if (get_equality_with_lower()) { - return findLower(time); + return calcLower(time); } else { OPENSIM_THROW_IF_FRMOBJ(getProperty_upper_bound().empty(), Exception, "No upper bound provided."); diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 95663cda1..23eedc835 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -203,14 +203,22 @@ class MocoCasOCProblem : public CasOC::Problem { casadi::DM findTimeVaryingStateLowerBoundsImpl( const std::string& name, const casadi::DM& times) const override { auto mocoProblemRep = m_jar->take(); + casadi::DM ret(casadi::Sparsity::dense(times.rows(), times.columns())); for (int i = 0; i < times.numel(); ++i) { - mocoProblemRep->getState + ret(i, 0) = mocoProblemRep->calcStateLowerFunctionBound(times(i, 0).scalar()); } - + return ret; + m_jar->leave(std::move(mocoProblemRep)); } casadi::DM findTimeVaryingStateUpperBoundsImpl( const std::string& name, const casadi::DM& times) const override { - + auto mocoProblemRep = m_jar->take(); + casadi::DM ret(casadi::Sparsity::dense(times.rows(), times.columns())); + for (int i = 0; i < times.numel(); ++i) { + ret(i, 0) = mocoProblemRep->calcStateUpperFunctionBound(times(i, 0).scalar()); + } + return ret; + m_jar->leave(std::move(mocoProblemRep)); }; void calcMultibodySystemExplicit(const ContinuousInput& input, bool calcKCErrors, diff --git a/Moco/Moco/MocoVariableInfo.h b/Moco/Moco/MocoVariableInfo.h index bf0cc2344..ac13bb6b4 100644 --- a/Moco/Moco/MocoVariableInfo.h +++ b/Moco/Moco/MocoVariableInfo.h @@ -40,6 +40,18 @@ class OSIMMOCO_API MocoVariableInfo : public Object { /// TODO bool getUseFunctionBounds() const { return get_use_function_bounds(); } + double calcLowerFunctionBound(const double& time) const { + OPENSIM_THROW_IF_FRMOBJ(!getUseFunctionBounds(), Exception, + "This info is not set to use function bounds."); + return get_function_bounds().calcLower(time); + } + + double calcUpperFunctionBound(const double& time) const { + OPENSIM_THROW_IF_FRMOBJ(!getUseFunctionBounds(), Exception, + "This info is not set to use function bounds."); + return get_function_bounds().calcUpper(time); + } + /// @details Note: the return value is constructed fresh on every call from /// the internal property. Avoid repeated calls to this function. MocoBounds getPhaseBounds() const { From 7243962d9111e6d22a2197a9bb66d92fa8f41e06 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Tue, 15 Oct 2019 17:32:46 -0700 Subject: [PATCH 4/8] FunctionBounds compiles. --- Moco/Moco/MocoBounds.h | 2 +- Moco/Moco/MocoCasADiSolver/CasOCProblem.h | 46 ++++--- .../MocoCasADiSolver/CasOCTranscription.cpp | 20 ++- .../MocoCasADiSolver/CasOCTranscription.h | 6 +- .../MocoCasADiSolver/MocoCasOCProblem.cpp | 6 +- Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h | 22 ++-- Moco/Moco/MocoProblemRep.cpp | 26 ++-- Moco/Moco/MocoTrack.cpp | 34 +++-- Moco/Moco/MocoTrack.h | 6 + Moco/Moco/MocoUtilities.cpp | 14 +- Moco/Moco/MocoUtilities.h | 17 +++ Moco/Moco/MocoVariableInfo.h | 14 +- Moco/Moco/tropter/TropterProblem.h | 6 +- Moco/Tests/testMocoInterface.cpp | 121 ++++++++++-------- 14 files changed, 195 insertions(+), 145 deletions(-) diff --git a/Moco/Moco/MocoBounds.h b/Moco/Moco/MocoBounds.h index da7ac42a8..c6a7f0516 100644 --- a/Moco/Moco/MocoBounds.h +++ b/Moco/Moco/MocoBounds.h @@ -135,7 +135,7 @@ class OSIMMOCO_API MocoFunctionBounds : public Object { } /// Returns true if the provided value is within these bounds. bool isWithinBounds(const double& time, const double& value) const { - return findLower(time) <= value && value <= findUpper(time); + return calcLower(time) <= value && value <= calcUpper(time); } double calcLower(const double& time) const { SimTK::Vector timeVec(1, &time, true); diff --git a/Moco/Moco/MocoCasADiSolver/CasOCProblem.h b/Moco/Moco/MocoCasADiSolver/CasOCProblem.h index 3dac83b39..b47aa9de5 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/Moco/Moco/MocoCasADiSolver/CasOCProblem.h @@ -304,19 +304,23 @@ class Problem { } public: - casadi::DM findTimeVaryingStateLowerBounds(const std::string& name, - const casadi::DM& times) const { - checkStateNameForTimeVaryingBounds(name); - return findTimeVaryingStateLowerBoundsImpl(name, times); - } - casadi::DM findTimeVaryingStateUpperBounds(const std::string& name, + struct TimeVaryingBounds { + casadi::DM lower; + casadi::DM upper; + }; + TimeVaryingBounds findTimeVaryingStateBounds(const std::string& name, const casadi::DM& times) const { - checkStateNameForTimeVaryingBounds(name); - return findTimeVaryingStateUpperBoundsImpl(name, times); + 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 findTimeVaryingStateBounds(name, times); } - virtual casadi::DM findTimeVaryingStateLowerBoundsImpl( - const std::string& name, const casadi::DM& times) const = 0; - virtual casadi::DM findTimeVaryingStateUpperBoundsImpl( + virtual TimeVaryingBounds findTimeVaryingStateBoundsImpl( const std::string& name, const casadi::DM& times) const = 0; /// Kinematic constraint errors should be ordered as so: /// - position-level constraints @@ -405,6 +409,7 @@ class Problem { void initialize(const std::string& finiteDiffScheme, std::shared_ptr> pointsForSparsityDetection) const { + OPENSIM_THROW_IF(m_usingTimeVaryingBounds && !isFixedTime(), OpenSim::Exception, "Cannot use time-varying bounds for non-fixed-time problems."); @@ -575,6 +580,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& getStateInfos() const { return m_stateInfos; } @@ -638,15 +653,6 @@ class Problem { endpoint.lower = std::max(b.lower, endpoint.lower); endpoint.upper = std::min(b.upper, endpoint.upper); } - void checkStateNameForTimeVaryingBounds(const std::string& name) 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."); - } Bounds m_timeInitialBounds; Bounds m_timeFinalBounds; diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 0753d0fe9..6bcfea59a 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -184,19 +184,25 @@ 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) { if (info.usingFunctionBounds) { - const casadi::DM lower = - m_problem.findTimeVaryingStateLowerBounds(info.name, - m_times); - const casadi::DM upper = - m_problem.findTimeVaryingStateUpperBounds(info.name, - m_times); + const Problem::TimeVaryingBounds bounds = + m_problem.findTimeVaryingStateBounds( + info.name, timesDM); for (int itime = 0; itime < m_numGridPoints; ++itime) { setVariableBounds(states, is, itime, - {lower(itime, 0), upper(itime, 0)}); + {bounds.lower(itime, 0).scalar(), + bounds.upper(itime, 0).scalar()}); } } else { diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h index a5c55e181..f74202c82 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/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::infinity(); diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 5620217ea..6080972e4 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -58,7 +58,7 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, if (info.getUseFunctionBounds()) { addStateWithTimeVaryingBounds(stateName, stateType); } else { - addState(stateName, stateType, convertBounds(info.getBounds()), + addState(stateName, stateType, convertBounds(info.getPhaseBounds()), convertBounds(info.getInitialBounds()), convertBounds(info.getFinalBounds())); } @@ -68,7 +68,7 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, 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())); } @@ -173,7 +173,7 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, } addKinematicConstraint(multInfo.getName(), - convertBounds(multInfo.getBounds()), + convertBounds(multInfo.getPhaseBounds()), convertBounds(multInfo.getInitialBounds()), convertBounds(multInfo.getFinalBounds()), kinLevel); diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 23eedc835..ef0968217 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -200,26 +200,20 @@ class MocoCasOCProblem : public CasOC::Problem { int getJarSize() const { return (int)m_jar->size(); } private: - casadi::DM findTimeVaryingStateLowerBoundsImpl( + TimeVaryingBounds findTimeVaryingStateBoundsImpl( const std::string& name, const casadi::DM& times) const override { auto mocoProblemRep = m_jar->take(); - casadi::DM ret(casadi::Sparsity::dense(times.rows(), times.columns())); + 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) { - ret(i, 0) = mocoProblemRep->calcStateLowerFunctionBound(times(i, 0).scalar()); + L(i, 0) = bounds.calcLower(times(i, 0).scalar()); + U(i, 0) = bounds.calcUpper(times(i, 0).scalar()); } - return ret; m_jar->leave(std::move(mocoProblemRep)); + return {std::move(L), std::move(U)}; } - casadi::DM findTimeVaryingStateUpperBoundsImpl( - const std::string& name, const casadi::DM& times) const override { - auto mocoProblemRep = m_jar->take(); - casadi::DM ret(casadi::Sparsity::dense(times.rows(), times.columns())); - for (int i = 0; i < times.numel(); ++i) { - ret(i, 0) = mocoProblemRep->calcStateUpperFunctionBound(times(i, 0).scalar()); - } - return ret; - m_jar->leave(std::move(mocoProblemRep)); - }; void calcMultibodySystemExplicit(const ContinuousInput& input, bool calcKCErrors, MultibodySystemExplicitOutput& output) const override { diff --git a/Moco/Moco/MocoProblemRep.cpp b/Moco/Moco/MocoProblemRep.cpp index e0e1aa25e..9668e0a9b 100644 --- a/Moco/Moco/MocoProblemRep.cpp +++ b/Moco/Moco/MocoProblemRep.cpp @@ -310,10 +310,10 @@ void MocoProblemRep::initialize() { const auto info = MocoVariableInfo(statePath, {}, {}, {}); m_state_infos[statePath] = info; } - if (!m_state_infos[statePath].getBounds().isSet()) { + if (!m_state_infos[statePath].getPhaseBounds().isSet()) { const auto& bounds = output->getValue(m_state_base); - m_state_infos[statePath].setBounds({bounds[0], bounds[1]}); + m_state_infos[statePath].setPhaseBounds({bounds[0], bounds[1]}); } } } @@ -330,8 +330,8 @@ void MocoProblemRep::initialize() { MocoVariableInfo(coordValueName, {}, {}, {}); m_state_infos[coordValueName] = info; } - if (!m_state_infos[coordValueName].getBounds().isSet()) { - m_state_infos[coordValueName].setBounds( + if (!m_state_infos[coordValueName].getPhaseBounds().isSet()) { + m_state_infos[coordValueName].setPhaseBounds( {coord.getRangeMin(), coord.getRangeMax()}); } } @@ -342,8 +342,8 @@ void MocoProblemRep::initialize() { MocoVariableInfo(coordSpeedName, {}, {}, {}); m_state_infos[coordSpeedName] = info; } - if (!m_state_infos[coordSpeedName].getBounds().isSet()) { - m_state_infos[coordSpeedName].setBounds( + if (!m_state_infos[coordSpeedName].getPhaseBounds().isSet()) { + m_state_infos[coordSpeedName].setPhaseBounds( ph0.get_default_speed_bounds()); } } @@ -389,17 +389,17 @@ void MocoProblemRep::initialize() { const auto info = MocoVariableInfo(actuName, {}, {}, {}); m_control_infos[actuName] = info; } - if (!m_control_infos[actuName].getBounds().isSet()) { + if (!m_control_infos[actuName].getPhaseBounds().isSet()) { // If this scalar actuator derives from OpenSim::ScalarActuator, // use the getMinControl() and getMaxControl() methods to set // the bounds. Otherwise, set the bounds to (-inf, inf). if (const auto* scalarActu = dynamic_cast(&actu)) { - m_control_infos[actuName].setBounds( + m_control_infos[actuName].setPhaseBounds( {scalarActu->getMinControl(), scalarActu->getMaxControl()}); } else { - m_control_infos[actuName].setBounds( + m_control_infos[actuName].setPhaseBounds( MocoBounds::unconstrained()); } } @@ -410,8 +410,8 @@ void MocoProblemRep::initialize() { const std::string stateName = actuName + "/activation"; auto& info = m_state_infos[stateName]; if (info.getName().empty()) { info.setName(stateName); } - if (!info.getBounds().isSet()) { - info.setBounds(m_control_infos[actuName].getBounds()); + if (!info.getPhaseBounds().isSet()) { + info.setPhaseBounds(m_control_infos[actuName].getPhaseBounds()); } } } @@ -425,8 +425,8 @@ void MocoProblemRep::initialize() { const auto info = MocoVariableInfo(controlName, {}, {}, {}); m_control_infos[controlName] = info; } - if (!m_control_infos[controlName].getBounds().isSet()) { - m_control_infos[controlName].setBounds( + if (!m_control_infos[controlName].getPhaseBounds().isSet()) { + m_control_infos[controlName].setPhaseBounds( MocoBounds::unconstrained()); } } diff --git a/Moco/Moco/MocoTrack.cpp b/Moco/Moco/MocoTrack.cpp index a801ce935..519b3e565 100644 --- a/Moco/Moco/MocoTrack.cpp +++ b/Moco/Moco/MocoTrack.cpp @@ -47,6 +47,10 @@ void MocoTrack::constructProperties() { constructProperty_allow_unused_references(false); constructProperty_guess_file(""); constructProperty_apply_tracked_states_to_guess(false); + constructProperty_bound_kinematic_states(); + constructProperty_state_bound_range_rotational( + SimTK::convertDegreesToRadians(20)); + constructProperty_state_bound_range_translational(0.20); constructProperty_minimize_control_effort(true); constructProperty_control_effort_weight(0.001); } @@ -102,21 +106,31 @@ MocoStudy MocoTrack::initialize() { } problem.setTimeBounds(m_timeInfo.initial, m_timeInfo.final); - // Set state bounds. - // ----------------- - // TODO: Default true. Make it an optional property. - if (get_bound_kinematic_states_with_states_reference()) { - OPENSIM_THROW_IF_FRMOBJ(get_states_reference().empty(), + // Bound kinematic states. + // ----------------------- + bool boundKinematicStates = false; + if (getProperty_bound_kinematic_states().empty()) { + if (!get_states_reference().empty()) { + boundKinematicStates = true; + } + } else { + OPENSIM_THROW_IF_FRMOBJ( + get_bound_kinematic_states() && get_states_reference().empty(), Exception, "Cannot bound kinematic states with states reference if no " "states reference is supplied."); + } + + checkPropertyIsNonnegative( + *this, getProperty_state_bound_range_rotational()); + checkPropertyIsNonnegative( + *this, getProperty_state_bound_range_translational()); + + if (boundKinematicStates) { setKinematicStateFunctionBoundsFromTable(model, tracked_states, - // TODO: use properties for these ranges. - SimTK::convertDegreesToRadians(10), 0.10, - problem); - // get_state_bound_range_rotational(), - // get_state_bound_range_translational()); + get_state_bound_range_rotational(), + get_state_bound_range_translational(), problem); } diff --git a/Moco/Moco/MocoTrack.h b/Moco/Moco/MocoTrack.h index 0a2b51c7d..2081a34ac 100644 --- a/Moco/Moco/MocoTrack.h +++ b/Moco/Moco/MocoTrack.h @@ -227,6 +227,12 @@ class OSIMMOCO_API MocoTrack : public MocoTool { "This " "will override any guess information provided via `guess_file`."); + OpenSim_DECLARE_OPTIONAL_PROPERTY(bound_kinematic_states, bool, "TODO"); + + OpenSim_DECLARE_PROPERTY(state_bound_range_rotational, double, "TODO"); + + OpenSim_DECLARE_PROPERTY(state_bound_range_translational, double, "TODO"); + OpenSim_DECLARE_PROPERTY(minimize_control_effort, bool, "Whether or not to minimize actuator control effort in the problem." "Default: true."); diff --git a/Moco/Moco/MocoUtilities.cpp b/Moco/Moco/MocoUtilities.cpp index 6fd43c9cf..6fb9d79b1 100644 --- a/Moco/Moco/MocoUtilities.cpp +++ b/Moco/Moco/MocoUtilities.cpp @@ -787,9 +787,9 @@ void OpenSim::setKinematicStateFunctionBoundsFromTable(const Model& model, const auto coords = model.getCoordinatesInMultibodyTreeOrder(); const int numRows = (int)kinematics.getNumRows(); const auto& time = kinematics.getIndependentColumn(); - const std::unordered_map ranges = - {{Coordinate::Rotational, rangeRotational}, - {Coordinate::Translational, rangeTranslational}}; + const std::map ranges = { + {Coordinate::Rotational, rangeRotational}, + {Coordinate::Translational, rangeTranslational}}; for (const auto& coord : coords) { const auto& motionType = coord->getMotionType(); // We can't set the bounds for coupled coordinates in a generic way, @@ -799,10 +799,10 @@ void OpenSim::setKinematicStateFunctionBoundsFromTable(const Model& model, const auto& valueStr = coord->getStateVariableNames().get(0); if (kinematics.hasColumn(valueStr)) { const auto& column = kinematics.getDependentColumn(valueStr); - SimTK::Vector temp = column - ranges[motionType]; + SimTK::Vector temp = column - ranges.at(motionType); GCVSpline lower(5, numRows, time.data(), temp.getContiguousScalarData()); - temp = column + ranges[motionType]; + temp = column + ranges.at(motionType); GCVSpline upper(5, numRows, time.data(), temp.getContiguousScalarData()); problem.setStateInfo(valueStr, @@ -811,10 +811,10 @@ void OpenSim::setKinematicStateFunctionBoundsFromTable(const Model& model, const auto& speedStr = coord->getStateVariableNames().get(1); if (kinematics.hasColumn(speedStr)) { const auto& column = kinematics.getDependentColumn(speedStr); - SimTK::Vector temp = column - ranges[motionType]; + SimTK::Vector temp = column - ranges.at(motionType); GCVSpline lower(5, numRows, time.data(), temp.getContiguousScalarData()); - temp = column + ranges[motionType]; + temp = column + ranges.at(motionType); GCVSpline upper(5, numRows, time.data(), temp.getContiguousScalarData()); problem.setStateInfo(speedStr, diff --git a/Moco/Moco/MocoUtilities.h b/Moco/Moco/MocoUtilities.h index 6029f5ebd..6fcf9cadf 100644 --- a/Moco/Moco/MocoUtilities.h +++ b/Moco/Moco/MocoUtilities.h @@ -565,6 +565,23 @@ void checkPropertyIsPositive(const Object& obj, const Property& p) { } } +/// Throw an exception if the property's value is negative. +/// We assume that `p` is a single-value property. +template +void checkPropertyIsNonnegative(const Object& obj, const Property& p) { + const auto& value = p.getValue(); + if (value < 0) { + std::stringstream msg; + msg << "Property '" << p.getName() << "' (in "; + if (!obj.getName().empty()) { + msg << "object '" << obj.getName() << "' of type "; + } + msg << obj.getConcreteClassName() << ") must be non-negative, but is " + << value << "."; + OPENSIM_THROW(Exception, msg.str()); + } +} + /// Throw an exception if the property's value is neither in the provided /// range nor in the provided set. /// We assume that `p` is a single-value property. diff --git a/Moco/Moco/MocoVariableInfo.h b/Moco/Moco/MocoVariableInfo.h index ac13bb6b4..be9b7f12b 100644 --- a/Moco/Moco/MocoVariableInfo.h +++ b/Moco/Moco/MocoVariableInfo.h @@ -40,16 +40,10 @@ class OSIMMOCO_API MocoVariableInfo : public Object { /// TODO bool getUseFunctionBounds() const { return get_use_function_bounds(); } - double calcLowerFunctionBound(const double& time) const { - OPENSIM_THROW_IF_FRMOBJ(!getUseFunctionBounds(), Exception, - "This info is not set to use function bounds."); - return get_function_bounds().calcLower(time); - } - - double calcUpperFunctionBound(const double& time) const { - OPENSIM_THROW_IF_FRMOBJ(!getUseFunctionBounds(), Exception, - "This info is not set to use function bounds."); - return get_function_bounds().calcUpper(time); + const MocoFunctionBounds& getFunctionBounds() const { + OPENSIM_THROW_IF_FRMOBJ(getProperty_function_bounds().empty(), + Exception, "This info does not have function bounds."); + return get_function_bounds(); } /// @details Note: the return value is constructed fresh on every call from diff --git a/Moco/Moco/tropter/TropterProblem.h b/Moco/Moco/tropter/TropterProblem.h index bbdc4d9ec..c23699bc8 100644 --- a/Moco/Moco/tropter/TropterProblem.h +++ b/Moco/Moco/tropter/TropterProblem.h @@ -95,7 +95,7 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { convertBounds(m_mocoProbRep.getTimeFinalBounds())); for (const auto& svName : m_svNamesInSysOrder) { const auto& info = m_mocoProbRep.getStateInfo(svName); - this->add_state(svName, convertBounds(info.getBounds()), + this->add_state(svName, convertBounds(info.getPhaseBounds()), convertBounds(info.getInitialBounds()), convertBounds(info.getFinalBounds())); } @@ -106,7 +106,7 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { createControlNamesFromModel(m_modelBase, m_modelControlIndices); for (const auto& controlName : controlNames) { const auto& info = m_mocoProbRep.getControlInfo(controlName); - this->add_control(controlName, convertBounds(info.getBounds()), + this->add_control(controlName, convertBounds(info.getPhaseBounds()), convertBounds(info.getInitialBounds()), convertBounds(info.getFinalBounds())); } @@ -215,7 +215,7 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { const auto& multInfo = multInfos[multIndexThisConstraint]; this->add_adjunct(multInfo.getName(), - convertBounds(multInfo.getBounds()), + convertBounds(multInfo.getPhaseBounds()), convertBounds(multInfo.getInitialBounds()), convertBounds(multInfo.getFinalBounds())); // Add velocity correction variables if enforcing diff --git a/Moco/Tests/testMocoInterface.cpp b/Moco/Tests/testMocoInterface.cpp index e98f2b7f8..fe1357b62 100644 --- a/Moco/Tests/testMocoInterface.cpp +++ b/Moco/Tests/testMocoInterface.cpp @@ -456,14 +456,14 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { MocoProblemRep rep = problem.createRep(); { const auto& info = rep.getStateInfo("/slider/position/value"); - SimTK_TEST_EQ(info.getBounds().getLower(), -10); - SimTK_TEST_EQ(info.getBounds().getUpper(), 15); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), -10); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 15); } { // Default speed bounds. const auto& info = rep.getStateInfo("/slider/position/speed"); - SimTK_TEST_EQ(info.getBounds().getLower(), -50); - SimTK_TEST_EQ(info.getBounds().getUpper(), 50); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), -50); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 50); } // No control info stored in the Problem. SimTK_TEST_MUST_THROW_EXC( @@ -471,8 +471,8 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { { // Obtained from controls. const auto& info = rep.getControlInfo("/actuator"); - SimTK_TEST_EQ(info.getBounds().getLower(), 35); - SimTK_TEST_EQ(info.getBounds().getUpper(), 56); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), 35); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 56); } } @@ -480,14 +480,14 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { SECTION("Setting control info explicitly") { { const auto& probinfo = phase0.getControlInfo("/actuator"); - SimTK_TEST_EQ(probinfo.getBounds().getLower(), 12); - SimTK_TEST_EQ(probinfo.getBounds().getUpper(), 15); + SimTK_TEST_EQ(probinfo.getPhaseBounds().getLower(), 12); + SimTK_TEST_EQ(probinfo.getPhaseBounds().getUpper(), 15); } { MocoProblemRep rep = problem.createRep(); const auto& info = rep.getControlInfo("/actuator"); - SimTK_TEST_EQ(info.getBounds().getLower(), 12); - SimTK_TEST_EQ(info.getBounds().getUpper(), 15); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), 12); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 15); } } @@ -496,20 +496,20 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { { { const auto& probinfo0 = phase0.getControlInfo("/residuals_0"); - SimTK_TEST_EQ(probinfo0.getBounds().getLower(), -5); - SimTK_TEST_EQ(probinfo0.getBounds().getUpper(), 5); + SimTK_TEST_EQ(probinfo0.getPhaseBounds().getLower(), -5); + SimTK_TEST_EQ(probinfo0.getPhaseBounds().getUpper(), 5); const auto& probinfo3 = phase0.getControlInfo("/residuals_3"); - SimTK_TEST_EQ(probinfo3.getBounds().getLower(), -7.5); - SimTK_TEST_EQ(probinfo3.getBounds().getUpper(), 10); + SimTK_TEST_EQ(probinfo3.getPhaseBounds().getLower(), -7.5); + SimTK_TEST_EQ(probinfo3.getPhaseBounds().getUpper(), 10); } MocoProblemRep rep = problem.createRep(); { const auto& info0 = rep.getControlInfo("/residuals_0"); - SimTK_TEST_EQ(info0.getBounds().getLower(), -5); - SimTK_TEST_EQ(info0.getBounds().getUpper(), 5); + SimTK_TEST_EQ(info0.getPhaseBounds().getLower(), -5); + SimTK_TEST_EQ(info0.getPhaseBounds().getUpper(), 5); const auto& info3 = rep.getControlInfo("/residuals_3"); - SimTK_TEST_EQ(info3.getBounds().getLower(), -7.5); - SimTK_TEST_EQ(info3.getBounds().getUpper(), 10); + SimTK_TEST_EQ(info3.getPhaseBounds().getLower(), -7.5); + SimTK_TEST_EQ(info3.getPhaseBounds().getUpper(), 10); } } @@ -519,15 +519,15 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { { const auto& info = phase0.getStateInfo("/slider/position/value"); - SimTK_TEST(!info.getBounds().isSet()); + SimTK_TEST(!info.getPhaseBounds().isSet()); SimTK_TEST_EQ(info.getInitialBounds().getLower(), -5.0); SimTK_TEST_EQ(info.getInitialBounds().getUpper(), 3.6); SimTK_TEST(!info.getFinalBounds().isSet()); } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getStateInfo("/slider/position/value"); - SimTK_TEST_EQ(info.getBounds().getLower(), -10); - SimTK_TEST_EQ(info.getBounds().getUpper(), 15); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), -10); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 15); SimTK_TEST_EQ(info.getInitialBounds().getLower(), -5.0); SimTK_TEST_EQ(info.getInitialBounds().getUpper(), 3.6); SimTK_TEST(!info.getFinalBounds().isSet()); @@ -538,15 +538,15 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { { const auto& info = phase0.getStateInfo("/slider/position/value"); - SimTK_TEST(!info.getBounds().isSet()); + SimTK_TEST(!info.getPhaseBounds().isSet()); SimTK_TEST(!info.getInitialBounds().isSet()); SimTK_TEST_EQ(info.getFinalBounds().getLower(), 1.3); SimTK_TEST_EQ(info.getFinalBounds().getUpper(), 2.5); } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getStateInfo("/slider/position/value"); - SimTK_TEST_EQ(info.getBounds().getLower(), -10); - SimTK_TEST_EQ(info.getBounds().getUpper(), 15); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), -10); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 15); SimTK_TEST(!info.getInitialBounds().isSet()); SimTK_TEST_EQ(info.getFinalBounds().getLower(), 1.3); SimTK_TEST_EQ(info.getFinalBounds().getUpper(), 2.5); @@ -556,15 +556,15 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { { const auto& info = phase0.getStateInfo("/slider/position/speed"); - SimTK_TEST(!info.getBounds().isSet()); + SimTK_TEST(!info.getPhaseBounds().isSet()); SimTK_TEST_EQ(info.getInitialBounds().getLower(), -4.1); SimTK_TEST_EQ(info.getInitialBounds().getUpper(), 3.9); SimTK_TEST(!info.getFinalBounds().isSet()); } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getStateInfo("/slider/position/speed"); - SimTK_TEST_EQ(info.getBounds().getLower(), -50); - SimTK_TEST_EQ(info.getBounds().getUpper(), 50); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), -50); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 50); SimTK_TEST_EQ(info.getInitialBounds().getLower(), -4.1); SimTK_TEST_EQ(info.getInitialBounds().getUpper(), 3.9); SimTK_TEST(!info.getFinalBounds().isSet()); @@ -575,15 +575,15 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { { const auto& info = phase0.getStateInfo("/slider/position/speed"); - SimTK_TEST(!info.getBounds().isSet()); + SimTK_TEST(!info.getPhaseBounds().isSet()); SimTK_TEST(!info.getInitialBounds().isSet()); SimTK_TEST_EQ(info.getFinalBounds().getLower(), 0.1); SimTK_TEST_EQ(info.getFinalBounds().getUpper(), 0.8); } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getStateInfo("/slider/position/speed"); - SimTK_TEST_EQ(info.getBounds().getLower(), -50); - SimTK_TEST_EQ(info.getBounds().getUpper(), 50); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), -50); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 50); SimTK_TEST(!info.getInitialBounds().isSet()); SimTK_TEST_EQ(info.getFinalBounds().getLower(), 0.1); SimTK_TEST_EQ(info.getFinalBounds().getUpper(), 0.8); @@ -592,15 +592,15 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { problem.setControlInfo("/actuator", {}, {-4.1, 3.9}); { const auto& info = phase0.getControlInfo("/actuator"); - SimTK_TEST(!info.getBounds().isSet()); + SimTK_TEST(!info.getPhaseBounds().isSet()); SimTK_TEST_EQ(info.getInitialBounds().getLower(), -4.1); SimTK_TEST_EQ(info.getInitialBounds().getUpper(), 3.9); SimTK_TEST(!info.getFinalBounds().isSet()); } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getControlInfo("/actuator"); - SimTK_TEST_EQ(info.getBounds().getLower(), 35); - SimTK_TEST_EQ(info.getBounds().getUpper(), 56); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), 35); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 56); SimTK_TEST_EQ(info.getInitialBounds().getLower(), -4.1); SimTK_TEST_EQ(info.getInitialBounds().getUpper(), 3.9); SimTK_TEST(!info.getFinalBounds().isSet()); @@ -609,15 +609,15 @@ TEMPLATE_TEST_CASE("Workflow", "", MocoTropterSolver, MocoCasADiSolver) { problem.setControlInfo("/actuator", {}, {}, {0.1, 0.8}); { const auto& info = phase0.getControlInfo("/actuator"); - SimTK_TEST(!info.getBounds().isSet()); + SimTK_TEST(!info.getPhaseBounds().isSet()); SimTK_TEST(!info.getInitialBounds().isSet()); SimTK_TEST_EQ(info.getFinalBounds().getLower(), 0.1); SimTK_TEST_EQ(info.getFinalBounds().getUpper(), 0.8); } MocoProblemRep rep = problem.createRep(); const auto& info = rep.getControlInfo("/actuator"); - SimTK_TEST_EQ(info.getBounds().getLower(), 35); - SimTK_TEST_EQ(info.getBounds().getUpper(), 56); + SimTK_TEST_EQ(info.getPhaseBounds().getLower(), 35); + SimTK_TEST_EQ(info.getPhaseBounds().getUpper(), 56); SimTK_TEST(!info.getInitialBounds().isSet()); SimTK_TEST_EQ(info.getFinalBounds().getLower(), 0.1); SimTK_TEST_EQ(info.getFinalBounds().getUpper(), 0.8); @@ -748,38 +748,38 @@ TEMPLATE_TEST_CASE("Set infos with regular expression", "", MocoCasADiSolver, problem.setStateInfoPattern(".*/speed", {3, 10}); MocoProblemRep problemRep = problem.createRep(); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j0/q0/value") - .getBounds() + .getPhaseBounds() .getLower(), 2); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j1/q1/value") - .getBounds() + .getPhaseBounds() .getLower(), 2); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j0/q0/speed") - .getBounds() + .getPhaseBounds() .getLower(), 3); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j1/q1/speed") - .getBounds() + .getPhaseBounds() .getLower(), 3); problem.setStateInfo("/jointset/j0/q0/value", {3, 10}); problem.setStateInfo("/jointset/j1/q1/speed", {4, 10}); problemRep = problem.createRep(); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j0/q0/value") - .getBounds() + .getPhaseBounds() .getLower(), 3); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j1/q1/value") - .getBounds() + .getPhaseBounds() .getLower(), 2); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j0/q0/speed") - .getBounds() + .getPhaseBounds() .getLower(), 3); SimTK_TEST_EQ(problemRep.getStateInfo("/jointset/j1/q1/speed") - .getBounds() + .getPhaseBounds() .getLower(), 4); } @@ -1850,28 +1850,34 @@ TEST_CASE("MocoPhase::bound_activation_from_excitation") { } SECTION("bound_activation_from_excitation is true") { auto rep = problem.createRep(); - CHECK(rep.getStateInfo("/muscle/activation").getBounds().getLower() == - 0); - CHECK(rep.getStateInfo("/muscle/activation").getBounds().getUpper() == - 1); + CHECK(rep.getStateInfo("/muscle/activation") + .getPhaseBounds() + .getLower() == 0); + CHECK(rep.getStateInfo("/muscle/activation") + .getPhaseBounds() + .getUpper() == 1); } SECTION("bound_activation_from_excitation is true; non-default min/max " "control") { musclePtr->setMinControl(0.35); musclePtr->setMaxControl(0.96); auto rep = problem.createRep(); - CHECK(rep.getStateInfo("/muscle/activation").getBounds().getLower() == - Approx(0.35)); - CHECK(rep.getStateInfo("/muscle/activation").getBounds().getUpper() == - Approx(0.96)); + CHECK(rep.getStateInfo("/muscle/activation") + .getPhaseBounds() + .getLower() == Approx(0.35)); + CHECK(rep.getStateInfo("/muscle/activation") + .getPhaseBounds() + .getUpper() == Approx(0.96)); } SECTION("Custom excitation bounds") { problem.setControlInfo("/muscle", {0.41, 0.63}); auto rep = problem.createRep(); - CHECK(rep.getStateInfo("/muscle/activation").getBounds().getLower() == - Approx(0.41)); - CHECK(rep.getStateInfo("/muscle/activation").getBounds().getUpper() == - Approx(0.63)); + CHECK(rep.getStateInfo("/muscle/activation") + .getPhaseBounds() + .getLower() == Approx(0.41)); + CHECK(rep.getStateInfo("/muscle/activation") + .getPhaseBounds() + .getUpper() == Approx(0.63)); } SECTION("ignore_activation_dynamics") { musclePtr->set_ignore_activation_dynamics(true); @@ -1882,3 +1888,6 @@ TEST_CASE("MocoPhase::bound_activation_from_excitation") { } } +// TODO: Test functions with lower > upper. +// TODO: test constraining a state exactly (equality_with_lower). + From 4e63ec70e7a46aea34a55e83b4f2ac13e375ee8d Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Tue, 15 Oct 2019 17:53:45 -0700 Subject: [PATCH 5/8] exampleMocoTrack runs without crash or exception. --- Moco/Moco/MocoBounds.cpp | 6 ++---- Moco/Moco/MocoCasADiSolver/CasOCProblem.h | 6 ++++-- Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 4 ++-- Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h | 4 ++-- Moco/Moco/MocoProblemRep.cpp | 13 ++++++++----- Moco/Moco/MocoUtilities.cpp | 4 ++-- Moco/Moco/MocoVariableInfo.cpp | 2 +- Moco/Moco/MocoVariableInfo.h | 6 ------ 8 files changed, 21 insertions(+), 24 deletions(-) diff --git a/Moco/Moco/MocoBounds.cpp b/Moco/Moco/MocoBounds.cpp index a67e0cef2..dd0aa1156 100644 --- a/Moco/Moco/MocoBounds.cpp +++ b/Moco/Moco/MocoBounds.cpp @@ -35,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.", @@ -88,11 +88,9 @@ MocoFunctionBounds::MocoFunctionBounds(const Function& lower, void MocoFunctionBounds::printDescription(std::ostream& stream) const { - // TODO: Print min and max of the functions. if (isEquality()) { stream << get_lower_bound().getConcreteClassName(); - } - else { + } else { const auto& lower = get_lower_bound().getConcreteClassName(); const auto& upper = get_upper_bound().getConcreteClassName(); stream << "[" << lower << ", " << upper << "]"; diff --git a/Moco/Moco/MocoCasADiSolver/CasOCProblem.h b/Moco/Moco/MocoCasADiSolver/CasOCProblem.h index b47aa9de5..b3b7c0247 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/Moco/Moco/MocoCasADiSolver/CasOCProblem.h @@ -318,10 +318,12 @@ class Problem { OPENSIM_THROW_IF(!it->usingFunctionBounds, OpenSim::Exception, "State is not using function bounds."); - return findTimeVaryingStateBounds(name, times); + return findTimeVaryingStateBoundsImpl(name, times); } virtual TimeVaryingBounds findTimeVaryingStateBoundsImpl( - const std::string& name, const casadi::DM& times) const = 0; + 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 diff --git a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 6bcfea59a..5324ae529 100644 --- a/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/Moco/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -201,8 +201,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, info.name, timesDM); for (int itime = 0; itime < m_numGridPoints; ++itime) { setVariableBounds(states, is, itime, - {bounds.lower(itime, 0).scalar(), - bounds.upper(itime, 0).scalar()}); + {bounds.lower(itime).scalar(), + bounds.upper(itime).scalar()}); } } else { diff --git a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h index ef0968217..c3a16295b 100644 --- a/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/Moco/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -208,8 +208,8 @@ class MocoCasOCProblem : public CasOC::Problem { 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, 0) = bounds.calcLower(times(i, 0).scalar()); - U(i, 0) = bounds.calcUpper(times(i, 0).scalar()); + 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)}; diff --git a/Moco/Moco/MocoProblemRep.cpp b/Moco/Moco/MocoProblemRep.cpp index 9668e0a9b..205d9cecc 100644 --- a/Moco/Moco/MocoProblemRep.cpp +++ b/Moco/Moco/MocoProblemRep.cpp @@ -330,8 +330,10 @@ void MocoProblemRep::initialize() { MocoVariableInfo(coordValueName, {}, {}, {}); m_state_infos[coordValueName] = info; } - if (!m_state_infos[coordValueName].getPhaseBounds().isSet()) { - m_state_infos[coordValueName].setPhaseBounds( + auto& info = m_state_infos[coordValueName]; + if (!info.getUseFunctionBounds() && + !info.getPhaseBounds().isSet()) { + info.setPhaseBounds( {coord.getRangeMin(), coord.getRangeMax()}); } } @@ -342,9 +344,10 @@ void MocoProblemRep::initialize() { MocoVariableInfo(coordSpeedName, {}, {}, {}); m_state_infos[coordSpeedName] = info; } - if (!m_state_infos[coordSpeedName].getPhaseBounds().isSet()) { - m_state_infos[coordSpeedName].setPhaseBounds( - ph0.get_default_speed_bounds()); + auto& info = m_state_infos[coordSpeedName]; + if (!info.getUseFunctionBounds() && + !info.getPhaseBounds().isSet()) { + info.setPhaseBounds(ph0.get_default_speed_bounds()); } } } diff --git a/Moco/Moco/MocoUtilities.cpp b/Moco/Moco/MocoUtilities.cpp index 6fb9d79b1..1e66240ec 100644 --- a/Moco/Moco/MocoUtilities.cpp +++ b/Moco/Moco/MocoUtilities.cpp @@ -796,7 +796,7 @@ void OpenSim::setKinematicStateFunctionBoundsFromTable(const Model& model, // so we ignore. if (motionType == Coordinate::Rotational || motionType == Coordinate::Translational) { - const auto& valueStr = coord->getStateVariableNames().get(0); + const std::string valueStr = coord->getStateVariableNames().get(0); if (kinematics.hasColumn(valueStr)) { const auto& column = kinematics.getDependentColumn(valueStr); SimTK::Vector temp = column - ranges.at(motionType); @@ -808,7 +808,7 @@ void OpenSim::setKinematicStateFunctionBoundsFromTable(const Model& model, problem.setStateInfo(valueStr, {std::move(lower), std::move(upper)}); } - const auto& speedStr = coord->getStateVariableNames().get(1); + const std::string speedStr = coord->getStateVariableNames().get(1); if (kinematics.hasColumn(speedStr)) { const auto& column = kinematics.getDependentColumn(speedStr); SimTK::Vector temp = column - ranges.at(motionType); diff --git a/Moco/Moco/MocoVariableInfo.cpp b/Moco/Moco/MocoVariableInfo.cpp index c9bcddf73..e2b7055f4 100644 --- a/Moco/Moco/MocoVariableInfo.cpp +++ b/Moco/Moco/MocoVariableInfo.cpp @@ -72,11 +72,11 @@ void MocoVariableInfo::validate() const { } void MocoVariableInfo::printDescription(std::ostream& stream) const { + stream << getName() << ". bounds: "; if (get_use_function_bounds()) { get_function_bounds().printDescription(stream); } else { const auto bounds = getPhaseBounds(); - stream << getName() << ". bounds: "; bounds.printDescription(stream); const auto initial = getInitialBounds(); if (initial.isSet()) { diff --git a/Moco/Moco/MocoVariableInfo.h b/Moco/Moco/MocoVariableInfo.h index be9b7f12b..ff2eb1d53 100644 --- a/Moco/Moco/MocoVariableInfo.h +++ b/Moco/Moco/MocoVariableInfo.h @@ -49,20 +49,14 @@ class OSIMMOCO_API MocoVariableInfo : public Object { /// @details Note: the return value is constructed fresh on every call from /// the internal property. Avoid repeated calls to this function. MocoBounds getPhaseBounds() const { - OPENSIM_THROW_IF_FRMOBJ(getUseFunctionBounds(), Exception, - "This info is set to use function bounds."); return MocoBounds(getProperty_phase_bounds()); } /// @copydoc getPhaseBounds() MocoInitialBounds getInitialBounds() const { - OPENSIM_THROW_IF_FRMOBJ(getUseFunctionBounds(), Exception, - "This info is set to use function bounds."); return MocoInitialBounds(getProperty_initial_bounds()); } /// @copydoc getPhaseBounds() MocoFinalBounds getFinalBounds() const { - OPENSIM_THROW_IF_FRMOBJ(getUseFunctionBounds(), Exception, - "This info is set to use function bounds."); return MocoFinalBounds(getProperty_final_bounds()); } From 8edef24be2f200747d4d88d071ff0bdf1ae76bf1 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Sun, 20 Oct 2019 19:13:39 -0700 Subject: [PATCH 6/8] Update CHANGELOG. --- CHANGELOG.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 6efeb4bdf..85878def3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,5 +1,4 @@ -- 2019-10-13: State and control bounds can be time-varying with - MocoCasADiSolver. +- 2019-10-13: State bounds can be time-varying with MocoCasADiSolver. - 2019-10-16: Fix a bug in ModOpscaleMaxIsometricForce, where the scale factor was not used properly. From d5ddde6c6f0189648d05e35d31f28f564ecb13d0 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Sun, 20 Oct 2019 19:13:59 -0700 Subject: [PATCH 7/8] Update date. --- CHANGELOG.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 85878def3..529420a29 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,4 @@ -- 2019-10-13: State bounds can be time-varying with MocoCasADiSolver. +- 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. From f4a8e6e4cc11d0e694a908d4609bb73dc9860ac6 Mon Sep 17 00:00:00 2001 From: Christopher Dembia Date: Fri, 25 Oct 2019 13:18:29 -0700 Subject: [PATCH 8/8] Change name of bound variables. --- Moco/Moco/MocoTrack.cpp | 9 +++++---- Moco/Moco/MocoTrack.h | 4 ++-- Moco/Moco/MocoUtilities.cpp | 12 ++++++------ Moco/Moco/MocoUtilities.h | 4 ++-- 4 files changed, 15 insertions(+), 14 deletions(-) diff --git a/Moco/Moco/MocoTrack.cpp b/Moco/Moco/MocoTrack.cpp index 519b3e565..4688eced9 100644 --- a/Moco/Moco/MocoTrack.cpp +++ b/Moco/Moco/MocoTrack.cpp @@ -119,18 +119,19 @@ MocoStudy MocoTrack::initialize() { Exception, "Cannot bound kinematic states with states reference if no " "states reference is supplied."); + boundKinematicStates = get_bound_kinematic_states(); } checkPropertyIsNonnegative( - *this, getProperty_state_bound_range_rotational()); + *this, getProperty_state_bound_halfrange_rotational()); checkPropertyIsNonnegative( - *this, getProperty_state_bound_range_translational()); + *this, getProperty_state_bound_halfrange_translational()); if (boundKinematicStates) { setKinematicStateFunctionBoundsFromTable(model, tracked_states, - get_state_bound_range_rotational(), - get_state_bound_range_translational(), problem); + get_state_bound_halfrange_rotational(), + get_state_bound_halfrange_translational(), problem); } diff --git a/Moco/Moco/MocoTrack.h b/Moco/Moco/MocoTrack.h index 2081a34ac..fd8eaa324 100644 --- a/Moco/Moco/MocoTrack.h +++ b/Moco/Moco/MocoTrack.h @@ -229,9 +229,9 @@ class OSIMMOCO_API MocoTrack : public MocoTool { OpenSim_DECLARE_OPTIONAL_PROPERTY(bound_kinematic_states, bool, "TODO"); - OpenSim_DECLARE_PROPERTY(state_bound_range_rotational, double, "TODO"); + OpenSim_DECLARE_PROPERTY(state_bound_halfrange_rotational, double, "TODO"); - OpenSim_DECLARE_PROPERTY(state_bound_range_translational, double, "TODO"); + OpenSim_DECLARE_PROPERTY(state_bound_halfrange_translational, double, "TODO"); OpenSim_DECLARE_PROPERTY(minimize_control_effort, bool, "Whether or not to minimize actuator control effort in the problem." diff --git a/Moco/Moco/MocoUtilities.cpp b/Moco/Moco/MocoUtilities.cpp index 1e66240ec..82b680d2c 100644 --- a/Moco/Moco/MocoUtilities.cpp +++ b/Moco/Moco/MocoUtilities.cpp @@ -773,23 +773,23 @@ TimeSeriesTable OpenSim::createExternalLoadsTableForGait(Model model, } void OpenSim::setKinematicStateFunctionBoundsFromTable(const Model& model, - const TimeSeriesTable& kinematics, const double& rangeRotational, - const double& rangeTranslational, + const TimeSeriesTable& kinematics, const double& halfrangeRotational, + const double& halfrangeTranslational, MocoProblem& problem) { OPENSIM_THROW_IF(rangeRotational < 0, Exception, - format("Expected rangeRotational to be non-negative, but got {}.", + format("Expected halfrangeRotational to be non-negative, but got {}.", rangeRotational)); OPENSIM_THROW_IF(rangeTranslational < 0, Exception, - format("Expected rangeTranslational to be non-negative, " + format("Expected halfrangeTranslational to be non-negative, " "but got {}.", rangeTranslational)); const auto coords = model.getCoordinatesInMultibodyTreeOrder(); const int numRows = (int)kinematics.getNumRows(); const auto& time = kinematics.getIndependentColumn(); const std::map ranges = { - {Coordinate::Rotational, rangeRotational}, - {Coordinate::Translational, rangeTranslational}}; + {Coordinate::Rotational, halfrangeRotational}, + {Coordinate::Translational, halfrangeTranslational}}; for (const auto& coord : coords) { const auto& motionType = coord->getMotionType(); // We can't set the bounds for coupled coordinates in a generic way, diff --git a/Moco/Moco/MocoUtilities.h b/Moco/Moco/MocoUtilities.h index 6fcf9cadf..0279e8975 100644 --- a/Moco/Moco/MocoUtilities.h +++ b/Moco/Moco/MocoUtilities.h @@ -792,8 +792,8 @@ TimeSeriesTable createExternalLoadsTableForGait(Model model, /// TODO OSIMMOCO_API void setKinematicStateFunctionBoundsFromTable(const Model& model, - const TimeSeriesTable& kinematics, const double& rangeRotational, - const double& rangeTranslational, MocoProblem& problem); + const TimeSeriesTable& kinematics, const double& halfrangeRotational, + const double& halfrangeTranslational, MocoProblem& problem); } // namespace OpenSim