From 439eb31dddaddc191b62dc845e69c85d33bba558 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 26 Jul 2024 16:53:40 -0700 Subject: [PATCH 01/38] Create time variables across the trajectory --- .../exampleSlidingMass/exampleSlidingMass.cpp | 1 + .../Moco/MocoCasADiSolver/CasOCFunction.cpp | 12 +- OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h | 14 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 7 +- .../MocoCasADiSolver/CasOCHermiteSimpson.h | 6 +- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 7 +- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 7 +- .../MocoCasADiSolver/CasOCLegendreGauss.h | 5 +- .../CasOCLegendreGaussRadau.cpp | 7 +- .../CasOCLegendreGaussRadau.h | 5 +- OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp | 8 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 119 ++++++---- .../MocoCasADiSolver/CasOCTranscription.h | 205 ++++++++++++------ .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 6 +- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 6 +- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 32 +-- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 34 +-- dependencies/CMakeLists.txt | 14 +- 18 files changed, 300 insertions(+), 195 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 17eb22f15c..46730195f2 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -103,6 +103,7 @@ int main() { // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); + solver.set_parallel(0); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index 53dec48350..4371833cc6 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -73,7 +73,7 @@ casadi::Sparsity calcJacobianSparsityWithPerturbation(const VectorDM& x0s, return combinedSparsity; } -casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, +casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, bool symmetric) const { using casadi::DM; using casadi::Slice; @@ -193,11 +193,11 @@ casadi::Sparsity Endpoint::get_sparsity_in(casadi_int i) { return casadi::Sparsity(0, 0); } } -casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, +casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const { using casadi::Slice; if (i == 0) { - return fullPoint.at(initial_time); + return fullPoint.at(times)(0); } else if (i == 1) { return fullPoint.at(states)(Slice(), 0); } else if (i == 2) { @@ -207,7 +207,7 @@ casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, } else if (i == 4) { return fullPoint.at(derivatives)(Slice(), 0); } else if (i == 5) { - return fullPoint.at(final_time); + return fullPoint.at(times)(-1); } else if (i == 6) { return fullPoint.at(states)(Slice(), -1); } else if (i == 7) { @@ -317,7 +317,7 @@ casadi::DM VelocityCorrection::getSubsetPoint( const int NMBS = m_casProblem->getNumStates() - m_casProblem->getNumAuxiliaryStates(); if (i == 0) { - return fullPoint.at(initial_time); + return fullPoint.at(times)(0); } else if (i == 1) { return fullPoint.at(states)(Slice(0, NMBS), itime); } else if (i == 2) { @@ -370,7 +370,7 @@ VectorDM MultibodySystemImplicit::eval( out[i] = casadi::DM(sparsity_out(i)); } - Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], + Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], out[3]}; m_casProblem->calcMultibodySystemImplicit(input, CalcKCErrors, output); return out; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index f816f34b46..07eec8a9b7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -61,7 +61,7 @@ class Function : public casadi::Callback { bool has_jac_sparsity(casadi_int oind, casadi_int iind) const override { return !m_fullPointsForSparsityDetection->empty(); } - casadi::Sparsity get_jac_sparsity(casadi_int oind, casadi_int iind, + casadi::Sparsity get_jac_sparsity(casadi_int oind, casadi_int iind, bool symmetric) const override; protected: @@ -69,7 +69,7 @@ class Function : public casadi::Callback { private: /// Here, "point" refers to a vector of all variables in the optimization - /// problem. This function returns a subset of the variables at a point for + /// problem. This function returns a subset of the variables at a point for /// a given input index. VectorDM getSubsetPointsForSparsityDetection(casadi_int iind) const { VectorDM out(m_fullPointsForSparsityDetection->size()); @@ -81,14 +81,14 @@ class Function : public casadi::Callback { } /// As of CasADi 3.6, callback functions need to return Jacobian sparsity - /// patterns for each pair of inputs and outputs. Therefore this function + /// patterns for each pair of inputs and outputs. Therefore this function /// returns a subset point for a given input index. - virtual casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + virtual casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const { int itime = 0; using casadi::Slice; if (i == 0) { - return fullPoint.at(initial_time); + return fullPoint.at(times)(0); } else if (i == 1) { return fullPoint.at(states)(Slice(), itime); } else if (i == 2) { @@ -233,7 +233,7 @@ class Endpoint : public Function { /// provided point, but applying the integrand function and quadrature /// scheme here is complicated. For simplicity, we provide the integral as /// 0. - casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const override; protected: @@ -297,7 +297,7 @@ class VelocityCorrection : public Function { casadi::Sparsity get_sparsity_in(casadi_int i) override final; casadi::Sparsity get_sparsity_out(casadi_int i) override final; VectorDM eval(const VectorDM& args) const override; - casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + casadi::DM getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index b5dd2ff97a..75dc44089f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -50,8 +50,9 @@ DM HermiteSimpson::createMeshIndicesImpl() const { return indices; } -void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void HermiteSimpson::calcDefectsImpl(const casadi::MX& times, + const casadi::MX& x, const casadi::MX& xdot, + casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); @@ -67,7 +68,7 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, time_mid = 2 * imesh + 1; time_ip1 = 2 * imesh + 2; - const auto h = m_times(time_ip1) - m_times(time_i); + const auto h = times(time_ip1) - times(time_i); const auto x_i = x(Slice(), time_i); const auto x_mid = x(Slice(), time_mid); const auto x_ip1 = x(Slice(), time_ip1); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 8c7f25f428..c018be51b4 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -63,15 +63,15 @@ class HermiteSimpson : public Transcription { } } } - createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), + createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), 3, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 365c604a75..d6150dea1b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -25,8 +25,10 @@ namespace CasOC { /// This enum describes the different types of optimization variables, and /// are the keys for the Variables map. enum Var { - initial_time, - final_time, + // initial_time, + // final_time, + // Time Variables. + times, /// Differential variables. states, /// Algebraic variables. @@ -54,7 +56,6 @@ using VariablesMX = Variables; /// This struct is used to obtain initial guesses. struct Iterate { VariablesDM variables; - casadi::DM times; std::vector state_names; std::vector control_names; std::vector multiplier_names; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 33db331af3..2a7415abdb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -54,14 +54,15 @@ DM LegendreGauss::createMeshIndicesImpl() const { return indices; } -void LegendreGauss::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void LegendreGauss::calcDefectsImpl(const casadi::MX& times, + const casadi::MX& x, const casadi::MX& xdot, + casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * (m_degree + 1); - const auto h = m_times(igrid + m_degree + 1) - m_times(igrid); + const auto h = times(igrid + m_degree + 1) - times(igrid); const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); const auto xdot_i = xdot(Slice(), Slice(igrid + 1, igrid + m_degree + 1)); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 493801633f..62e60508fd 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -105,14 +105,15 @@ class LegendreGauss : public Transcription { grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, (m_degree + 1) * m_problem.getNumStates(), + m_degree + 2, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 281a753987..bbee41839a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -52,14 +52,15 @@ DM LegendreGaussRadau::createMeshIndicesImpl() const { return indices; } -void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& times, + const casadi::MX& x, const casadi::MX& xdot, + casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; - const auto h = m_times(igrid + m_degree) - m_times(igrid); + const auto h = times(igrid + m_degree) - times(igrid); const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); const auto xdot_i = xdot(Slice(), Slice(igrid + 1, igrid + m_degree + 1)); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 191cb3990f..4cd7ccc8cd 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -107,14 +107,15 @@ class LegendreGaussRadau : public Transcription { grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, m_degree * m_problem.getNumStates(), + m_degree + 1, pointsForInterpControls); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp index 7bc3f4a456..8927ffb9f8 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp @@ -122,10 +122,10 @@ Solution Solver::solve(const Iterate& guess) const { if (m_sparsity_detection == "initial-guess") { // Interpolate the guess. Iterate guessCopy(guess); - const auto guessTimes = - transcription->createTimes(guessCopy.variables.at(initial_time), - guessCopy.variables.at(final_time)); - guessCopy = guessCopy.resample(guessTimes); + // const auto guessTimes = + // transcription->createTimes(guessCopy.variables.at(initial_time), + // guessCopy.variables.at(final_time)); + guessCopy = guessCopy.resample(guessCopy.variables.at(times)); pointsForSparsityDetection->push_back(guessCopy.variables); } else if (m_sparsity_detection == "random") { // Make sure the exact same sparsity pattern is used every time. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index a2738647a7..985b56b341 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -60,9 +60,6 @@ class NlpsolCallback : public casadi::Callback { if (m_callbackInterval > 0 && evalCount % m_callbackInterval == 0) { Iterate iterate = m_problem.createIterate(); iterate.variables = m_transcription.expandVariables(args.at(0)); - iterate.times = - m_transcription.createTimes(iterate.variables[initial_time], - iterate.variables[final_time]); iterate.iteration = evalCount; m_problem.intermediateCallbackWithIterate(iterate); } @@ -82,6 +79,7 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numPointsPerMeshInterval, const casadi::DM& pointsForInterpControls) { // Set the grid. // ------------- @@ -94,6 +92,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numMeshIntervals = m_numMeshPoints - 1; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; + m_numPointsPerMeshInterval = numPointsPerMeshInterval; m_pointsForInterpControls = pointsForInterpControls; m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() @@ -123,8 +122,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Create variables. // ----------------- - m_scaledVars[initial_time] = MX::sym("initial_time"); - m_scaledVars[final_time] = MX::sym("final_time"); + // m_scaledVars[initial_time] = MX::sym("initial_time"); + // m_scaledVars[final_time] = MX::sym("final_time"); + m_scaledVars[times] = MX::sym("times", 1, m_numGridPoints); m_scaledVars[states] = MX::sym("states", m_problem.getNumStates(), m_numGridPoints); m_scaledVars[controls] = @@ -189,11 +189,21 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, initializeScalingDM(m_shift); initializeScalingDM(m_scale); - setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); - setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); + // setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); + // setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); - setVariableScaling(initial_time, 0, 0, m_problem.getTimeInitialBounds()); - setVariableScaling(final_time, 0, 0, m_problem.getTimeFinalBounds()); + // setVariableScaling(initial_time, 0, 0, m_problem.getTimeInitialBounds()); + // setVariableScaling(final_time, 0, 0, m_problem.getTimeFinalBounds()); + + setVariableBounds(times, 0, 0, m_problem.getTimeInitialBounds()); + setVariableBounds(times, 0, -1, m_problem.getTimeFinalBounds()); + setVariableBounds(times, 0, Slice(1, m_numGridPoints - 1), + {m_problem.getTimeInitialBounds().lower, + m_problem.getTimeFinalBounds().upper}); + + setVariableScaling(times, Slice(), Slice(), + {m_problem.getTimeInitialBounds().lower, + m_problem.getTimeFinalBounds().upper}); { const auto& stateInfos = m_problem.getStateInfos(); @@ -274,14 +284,14 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } m_unscaledVars = unscaleVariables(m_scaledVars); - m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; - m_times = createTimes( - m_unscaledVars[initial_time], m_unscaledVars[final_time]); + m_duration = m_unscaledVars[times](-1) - m_unscaledVars[times](0); + // m_times = createTimes( + // m_unscaledVars[initial_time], m_unscaledVars[final_time]); m_paramsTrajGrid = MX::repmat(m_unscaledVars[parameters], 1, m_numGridPoints); m_paramsTrajMesh = MX::repmat(m_unscaledVars[parameters], 1, m_numMeshPoints); - m_paramsTrajMeshInterior = MX::repmat(m_unscaledVars[parameters], 1, + m_paramsTrajMeshInterior = MX::repmat(m_unscaledVars[parameters], 1, m_numMeshInteriorPoints); m_paramsTrajPathCon = MX::repmat(m_unscaledVars[parameters], 1, @@ -305,6 +315,12 @@ void Transcription::transcribe() { // TODO: Does creating all this memory have efficiency implications // in CasADi? + // Initialize memory for time constraints. + // --------------------------------------- + m_constraints.times = MX(casadi::Sparsity::dense(1, m_numGridPoints-2)); + m_constraintsLowerBounds.times = DM::zeros(1, m_numGridPoints-2); + m_constraintsUpperBounds.times = DM::zeros(1, m_numGridPoints-2); + // Initialize memory for state derivatives and defects. // ---------------------------------------------------- m_xdot = MX(NS, m_numGridPoints); @@ -346,6 +362,16 @@ void Transcription::transcribe() { m_constraintsUpperBounds.kinematic = casadi::DM::repmat( kcBounds.upper, numKinematicConstraints, m_numMeshPoints); + // time + // ---- + const auto& initialTime = m_unscaledVars[times](0); + const auto& finalTime = m_unscaledVars[times](-1); + for (int itime = 1; itime < m_numGridPoints - 1; ++itime) { + m_constraints.times(0, itime - 1) = + (finalTime - initialTime) * m_grid(itime) + initialTime + - m_unscaledVars[times](itime); + } + // qdot // ---- const MX u = m_unscaledVars[states](Slice(NQ, NQ + NU), Slice()); @@ -537,17 +563,17 @@ void Transcription::setObjectiveAndEndpointConstraints() { MXVector costOut; info.endpoint_function->call( - {m_unscaledVars[initial_time], + {m_unscaledVars[times](0), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), - m_unscaledVars[final_time], + m_unscaledVars[times](-1), m_unscaledVars[states](Slice(), -1), m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), - m_unscaledVars[parameters], + m_unscaledVars[parameters], integral}, costOut); m_objectiveTerms(iterm++) = casadi::MX::sum1(costOut.at(0)); @@ -611,12 +637,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { MXVector endpointOut; info.endpoint_function->call( - {m_unscaledVars[initial_time], + {m_unscaledVars[times](0), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), - m_unscaledVars[final_time], + m_unscaledVars[times](-1), m_unscaledVars[states](Slice(), -1), m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), @@ -638,9 +664,9 @@ Solution Transcription::solve(const Iterate& guessOrig) { // Resample the guess. // ------------------- - const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), - guessOrig.variables.at(final_time)); - auto guess = guessOrig.resample(guessTimes); + // const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), + // guessOrig.variables.at(final_time)); + auto guess = guessOrig.resample(guessOrig.variables.at(times)); // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). @@ -746,9 +772,6 @@ Solution Transcription::solve(const Iterate& guessOrig) { casadi::DMVector objectiveOut; objectiveFunc.call(finalVarsDMV, objectiveOut); solution.objective_breakdown = expandObjectiveTerms(objectiveOut[0]); - - solution.times = createTimes( - solution.variables[initial_time], solution.variables[final_time]); solution.stats = nlpFunc.stats(); // Print breakdown of objective. @@ -876,13 +899,13 @@ void Transcription::printConstraintValues(const Iterate& it, const auto& vars = it.variables; const auto& lower = m_lowerBounds; const auto& upper = m_upperBounds; - print_bounds("State bounds", it.state_names, it.times, vars.at(states), - lower.at(states), upper.at(states)); - print_bounds("Control bounds", it.control_names, it.times, vars.at(controls), - lower.at(controls), upper.at(controls)); - print_bounds("Multiplier bounds", it.multiplier_names, it.times, + print_bounds("State bounds", it.state_names, vars.at(times), + vars.at(states), lower.at(states), upper.at(states)); + print_bounds("Control bounds", it.control_names, vars.at(times), + vars.at(controls), lower.at(controls), upper.at(controls)); + print_bounds("Multiplier bounds", it.multiplier_names, vars.at(times), vars.at(multipliers), lower.at(multipliers), upper.at(multipliers)); - print_bounds("Derivative bounds", it.derivative_names, it.times, + print_bounds("Derivative bounds", it.derivative_names, vars.at(times), vars.at(derivatives), lower.at(derivatives), upper.at(derivatives)); // Need to update times for the slacks: // print_bounds("Slack bounds", it.slack_names, it.times, vars.at(slacks), @@ -969,16 +992,16 @@ void Transcription::printConstraintValues(const Iterate& it, } }; casadi::DM timeValues(2, 1); - timeValues(0) = vars.at(initial_time); - timeValues(1) = vars.at(final_time); + timeValues(0) = vars.at(times)(0); + timeValues(1) = vars.at(times)(-1); casadi::DM timeLower(2, 1); - timeLower(0) = lower.at(initial_time); - timeLower(1) = lower.at(final_time); + timeLower(0) = lower.at(times)(0); + timeLower(1) = lower.at(times)(-1); casadi::DM timeUpper(2, 1); - timeUpper(0) = upper.at(initial_time); - timeUpper(1) = upper.at(final_time); + timeUpper(0) = upper.at(times)(0); + timeUpper(1) = upper.at(times)(-1); printParameterBounds( "Time bounds", time_names, timeValues, timeLower, timeUpper); @@ -1017,7 +1040,7 @@ void Transcription::printConstraintValues(const Iterate& it, int argmax; double max = calcL1Norm(row, argmax); const double L1 = max; - const double time_of_max = it.times(argmax).scalar(); + const double time_of_max = vars.at(times)(argmax).scalar(); ss << std::setw(maxNameLength) << it.state_names[istate] << spacer << std::setprecision(2) << std::scientific << std::setw(9) << L2 @@ -1047,7 +1070,7 @@ void Transcription::printConstraintValues(const Iterate& it, int argmax; double max = calcL1Norm(row, argmax); const double L1 = max; - const double time_of_max = it.times(argmax).scalar(); + const double time_of_max = vars.at(times)(argmax).scalar(); std::string label = kinconNames.at(ikc); ss << std::setfill('0') << std::setw(2) << ikc << ":" @@ -1068,7 +1091,7 @@ void Transcription::printConstraintValues(const Iterate& it, for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { ss << std::setfill('0') << std::setw(3) << imesh << " "; ss.fill(' '); - ss << std::setw(9) << it.times(imesh).scalar() << " "; + ss << std::setw(9) << vars.at(times)(imesh).scalar() << " "; for (int ikc = 0; ikc < (int)kinconNames.size(); ++ikc) { const auto& value = constraints.kinematic(ikc, imesh).scalar(); ss << std::setprecision(2) << std::scientific @@ -1108,7 +1131,7 @@ void Transcription::printConstraintValues(const Iterate& it, int argmax; double max = calcL1Norm(row, argmax); const double L1 = max; - const double time_of_max = it.times(argmax).scalar(); + const double time_of_max = vars.at(times)(argmax).scalar(); std::string label = fmt::format("{}_{}", pc.name, ieq); ss << std::setfill('0') << std::setw(2) << ipc << ":" @@ -1130,7 +1153,7 @@ void Transcription::printConstraintValues(const Iterate& it, for (int ipcp = 0; ipcp < m_numPathConstraintPoints; ++ipcp) { ss << std::setfill('0') << std::setw(3) << ipcp << " "; ss.fill(' '); - ss << std::setw(9) << it.times(ipcp).scalar() << " "; + ss << std::setw(9) << vars.at(times)(ipcp).scalar() << " "; for (int ipc = 0; ipc < (int)pathconNames.size(); ++ipc) { const auto& value = constraints.path[ipc](ipcp).scalar(); ss << std::setprecision(2) << std::scientific @@ -1193,8 +1216,8 @@ Iterate Transcription::createInitialGuessFromBounds() const { setToMidpoint(kv.second, m_lowerBounds.at(kv.first), m_upperBounds.at(kv.first)); } - casGuess.times = createTimes( - casGuess.variables[initial_time], casGuess.variables[final_time]); + casGuess.variables[times] = createTimes( + casGuess.variables[times]); return casGuess; } @@ -1221,8 +1244,9 @@ Iterate Transcription::createRandomIterateWithinBounds( setRandom(kv.second, m_lowerBounds.at(kv.first), m_upperBounds.at(kv.first)); } - casIterate.times = createTimes(casIterate.variables[initial_time], - casIterate.variables[final_time]); + casIterate.variables[times] = createTimes( + casIterate.variables[times](0), + casIterate.variables[times](1)); return casIterate; } @@ -1236,12 +1260,13 @@ casadi::MXVector Transcription::evalOnTrajectory( // Assemble input. // Add 1 for time input and 1 for parameters input. MXVector mxIn(inputs.size() + 2); - mxIn[0] = m_times(timeIndices); + // mxIn[0] = m_times(timeIndices); + mxIn[0] = m_unscaledVars.at(times)(timeIndices); for (int i = 0; i < (int)inputs.size(); ++i) { if (inputs[i] == multibody_states) { const auto NQ = m_problem.getNumCoordinates(); const auto NU = m_problem.getNumSpeeds(); - mxIn[i + 1] = + mxIn[i + 1] = m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); } else if (inputs[i] == slacks) { mxIn[i + 1] = m_unscaledVars.at(inputs[i]); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 3e13b38ffb..89d7265be0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -38,8 +38,8 @@ class Transcription { Iterate createRandomIterateWithinBounds( const SimTK::Random* = nullptr) const; template - T createTimes(const T& initialTime, const T& finalTime) const { - return (finalTime - initialTime) * m_grid + initialTime; + T createTimes(const T& times) const { + return (times(-1) - times(0)) * m_grid + times(0); } casadi::DM createQuadratureCoefficients() const { return createQuadratureCoefficientsImpl(); @@ -73,6 +73,7 @@ class Transcription { /// scheme applies constraints between control points. void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numPointsPerMeshInterval, const casadi::DM& pointsForInterpControls = casadi::DM()); /// We assume all functions depend on time and parameters. @@ -123,6 +124,7 @@ class Transcription { template struct Constraints { + T times; T defects; T multibody_residuals; T auxiliary_residuals; @@ -145,13 +147,14 @@ class Transcription { int m_numMeshIntervals = -1; int m_numMeshInteriorPoints = -1; int m_numDefectsPerMeshInterval = -1; + int m_numPointsPerMeshInterval = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; casadi::DM m_grid; casadi::DM m_pointsForInterpControls; - casadi::MX m_times; + // casadi::MX m_times; casadi::MX m_duration; private: @@ -193,8 +196,8 @@ class Transcription { virtual casadi::DM createMeshIndicesImpl() const = 0; /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. - virtual void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const = 0; + virtual void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const = 0; virtual void calcInterpolatingControlsImpl(const casadi::MX& /*controls*/, casadi::MX& /*interpControls*/) const { OPENSIM_THROW_IF(m_pointsForInterpControls.numel(), OpenSim::Exception, @@ -204,8 +207,8 @@ class Transcription { void transcribe(); void setObjectiveAndEndpointConstraints(); void calcDefects() { - calcDefectsImpl( - m_unscaledVars.at(states), m_xdot, m_constraints.defects); + calcDefectsImpl(m_unscaledVars.at(times), m_unscaledVars.at(states), + m_xdot, m_constraints.defects); } void calcInterpolatingControls() { calcInterpolatingControlsImpl( @@ -307,84 +310,117 @@ class Transcription { // Trapezoidal sparsity pattern (mapping between flattened and expanded // constraints) for mesh intervals 0, 1 and 2: Endpoint constraints // depend on all time points through their integral. + // // 0 1 2 3 // endpoint x x x x - // path_0 x - // kinematic_0 x - // residual_0 x // defect_0 x x + // residual_0 x + // kinematic_0 x + // path_0 x + // defect_1 x x + // residual_1 x // kinematic_1 x // path_1 x - // residual_1 x - // defect_1 x x + // defect_2 x x + // residual_2 x // kinematic_2 x // path_2 x - // residual_2 x + // residual_3 x // kinematic_3 x // path_3 x - // residual_3 x - // Hermite-Simpson sparsity pattern for mesh intervals 0, 1 and 2 - // (* indicates additional non-zero entry when path constraint - // midpoints are enforced): + // Hermite-Simpson sparsity pattern for mesh intervals 0, 1 and 2. + // '*' indicates additional non-zero entry when path constraint + // mesh interior points are enforced. This sparsity pattern also applies + // to the Legendre-Gauss and Legendre-Gauss-Radau transcription with + // polynomial degree equal to 1. + // // 0 0.5 1 1.5 2 2.5 3 // endpoint x x x x x x x - // path_0 x * - // kinematic_0 x - // residual_0 x - // residual_0.5 x // defect_0 x x x + // residual_0 x x + // kinematic_0 x + // path_0 x * // interp_con_0 x x x + // defect_1 x x x + // residual_1 x x // kinematic_1 x // path_1 x * - // residual_1 x - // residual_1.5 x - // defect_1 x x x // interp_con_1 x x x + // defect_2 x x x + // residual_2 x x // kinematic_2 x // path_2 x * - // residual_2 x - // residual_2.5 x - // defect_2 x x x // interp_con_2 x x x + // residual_3 x // kinematic_3 x // path_3 x - // residual_3 x // 0 0.5 1 1.5 2 2.5 3 for (const auto& endpoint : constraints.endpoint) { copyColumn(endpoint, 0); } - for (int ipc = 0; ipc < m_numPathConstraintPoints; ++ipc) { - for (const auto& path: constraints.path) { - copyColumn(path, ipc); + // Constraints for each mesh interval. + int N = m_numPointsPerMeshInterval - 1; + int icon = 0; + int itime = 0; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + + // Time constraints. + for (int i = 0; i < N; ++i) { + if (igrid != 0 && igrid != m_numGridPoints - 1) { + copyColumn(constraints.times, itime++); + } } - } - int igrid = 0; - // Index for pointsForInterpControls. - int icon = 0; - for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { + // Defect constraints. + copyColumn(constraints.defects, imesh); + + // Multibody and auxiliary residuals. + for (int i = 0; i < N; ++i) { + copyColumn(constraints.multibody_residuals, igrid + i); + copyColumn(constraints.auxiliary_residuals, igrid + i); + } + + // Kinematic constraints. copyColumn(constraints.kinematic, imesh); - if (imesh < m_numMeshIntervals) { - while (m_grid(igrid).scalar() < m_solver.getMesh()[imesh + 1]) { - copyColumn(constraints.multibody_residuals, igrid); - copyColumn(constraints.auxiliary_residuals, igrid); - ++igrid; + + // Path constraints. + if (m_solver.getEnforcePathConstraintMidpoints()) { + for (int i = 0; i < N; ++i) { + for (const auto& path : constraints.path) { + copyColumn(path, igrid + i); + } + } + } else { + for (const auto& path : constraints.path) { + copyColumn(path, imesh); } - copyColumn(constraints.defects, imesh); - while (icon < m_pointsForInterpControls.numel() && - m_pointsForInterpControls(icon).scalar() < - m_solver.getMesh()[imesh + 1]) { - copyColumn(constraints.interp_controls, icon); - ++icon; + } + + // Interpolating controls. + if (m_pointsForInterpControls.numel()) { + for (int i = 0; i < N-1; ++i) { + copyColumn(constraints.interp_controls, icon++); } } } - // The loop above does not handle the residual at the final grid point. + + // Final grid point. copyColumn(constraints.multibody_residuals, m_numGridPoints - 1); copyColumn(constraints.auxiliary_residuals, m_numGridPoints - 1); + copyColumn(constraints.kinematic, m_numMeshPoints - 1); + if (m_solver.getEnforcePathConstraintMidpoints()) { + for (const auto& path : constraints.path) { + copyColumn(path, m_numGridPoints - 1); + } + } else { + for (const auto& path : constraints.path) { + copyColumn(path, m_numMeshPoints - 1); + } + } OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " @@ -403,7 +439,7 @@ class Transcription { }; Constraints out; out.defects = init(m_numDefectsPerMeshInterval, m_numMeshPoints - 1); - out.multibody_residuals = init(m_numMultibodyResiduals, + out.multibody_residuals = init(m_numMultibodyResiduals, m_numGridPoints); out.auxiliary_residuals = init(m_numAuxiliaryResiduals, m_numGridPoints); @@ -436,35 +472,66 @@ class Transcription { copyColumn(endpoint, 0); } - for (int ipc = 0; ipc < m_numPathConstraintPoints; ++ipc) { - for (auto& path : out.path) { - copyColumn(path, ipc); + // Constraints for each mesh interval. + int N = m_numPointsPerMeshInterval - 1; + int icon = 0; + int itime = 0; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + + // Time constraints. + for (int i = 0; i < N; ++i) { + if (igrid != 0 && igrid != m_numGridPoints - 1) { + copyColumn(out.times, itime++); + } } - } - int igrid = 0; - // Index for pointsForInterpControls. - int icon = 0; - for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { + // Defect constraints. + copyColumn(out.defects, imesh); + + // Multibody and auxiliary residuals. + for (int i = 0; i < N; ++i) { + copyColumn(out.multibody_residuals, igrid + i); + copyColumn(out.auxiliary_residuals, igrid + i); + } + + // Kinematic constraints. copyColumn(out.kinematic, imesh); - if (imesh < m_numMeshIntervals) { - while (m_grid(igrid).scalar() < m_solver.getMesh()[imesh + 1]) { - copyColumn(out.multibody_residuals, igrid); - copyColumn(out.auxiliary_residuals, igrid); - ++igrid; + + // Path constraints. + if (m_solver.getEnforcePathConstraintMidpoints()) { + for (int i = 0; i < N; ++i) { + for (auto& path : out.path) { + copyColumn(path, igrid + i); + } } - copyColumn(out.defects, imesh); - while (icon < m_pointsForInterpControls.numel() && - m_pointsForInterpControls(icon).scalar() < - m_solver.getMesh()[imesh + 1]) { - copyColumn(out.interp_controls, icon); - ++icon; + } else { + for (auto& path : out.path) { + copyColumn(path, imesh); + } + } + + // Interpolating controls. + if (m_pointsForInterpControls.numel()) { + for (int i = 0; i < N-1; ++i) { + copyColumn(out.interp_controls, icon++); } } } - // The loop above does not handle residuals at the final grid point. + + // Final grid point. copyColumn(out.multibody_residuals, m_numGridPoints - 1); copyColumn(out.auxiliary_residuals, m_numGridPoints - 1); + copyColumn(out.kinematic, m_numMeshPoints - 1); + if (m_solver.getEnforcePathConstraintMidpoints()) { + for (auto& path : out.path) { + copyColumn(path, m_numGridPoints - 1); + } + } else { + for (auto& path : out.path) { + copyColumn(path, m_numMeshPoints - 1); + } + } OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index a3834c2ea4..2655ec8d42 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -40,14 +40,14 @@ DM Trapezoidal::createMeshIndicesImpl() const { return DM::ones(1, m_numGridPoints); } -void Trapezoidal::calcDefectsImpl( - const casadi::MX& x, const casadi::MX& xdot, casadi::MX& defects) const { +void Trapezoidal::calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const { // We have arranged the code this way so that all constraints at a given // mesh point are grouped together (organizing the sparsity of the Jacobian // this way might have benefits for sparse linear algebra). for (int itime = 0; itime < m_numMeshIntervals; ++itime) { - const auto h = m_times(itime + 1) - m_times(itime); + const auto h = times(itime + 1) - times(itime); const auto x_i = x(Slice(), itime); const auto x_ip1 = x(Slice(), itime + 1); const auto xdot_i = xdot(Slice(), itime); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index b44367bc24..3aea1aafb9 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -35,15 +35,15 @@ class Trapezoidal : public Transcription { "Enforcing kinematic constraint derivatives " "not supported with trapezoidal transcription."); createVariablesAndSetBounds(m_solver.getMesh(), - m_problem.getNumStates()); + m_problem.getNumStates(), 2); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const override; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 1eca2adc09..84ef63d0ee 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -79,11 +79,11 @@ MocoTrajectory MocoCasADiSolver::createGuess(const std::string& type) const { auto casProblem = createCasOCProblem(); auto casSolver = createCasOCSolver(*casProblem); - std::vector inputControlIndexes = + std::vector inputControlIndexes = getProblemRep().getInputControlIndexes(); if (type == "bounds") { return convertToMocoTrajectory( - casSolver->createInitialGuessFromBounds(), + casSolver->createInitialGuessFromBounds(), inputControlIndexes); } else if (type == "random") { return convertToMocoTrajectory( @@ -368,7 +368,7 @@ MocoSolution MocoCasADiSolver::solveImpl() const { log_info("Number of threads: {}", casProblem->getJarSize()); } - std::vector inputControlIndexes = + std::vector inputControlIndexes = getProblemRep().getInputControlIndexes(); MocoTrajectory guess = getGuess(); CasOC::Iterate casGuess; @@ -383,19 +383,19 @@ MocoSolution MocoCasADiSolver::solveImpl() const { Logger::Level origLoggerLevel = Logger::getLevel(); Logger::setLevel(Logger::Level::Warn); CasOC::Solution casSolution; - try { - casSolution = casSolver->solve(casGuess); - } catch(const Exception& ex) { - OPENSIM_THROW_FRMOBJ(Exception, - fmt::format("MocoCasADiSolver failed internally with message: {}", - ex.getMessage())); - } catch(const casadi::CasadiException& ex) { - OPENSIM_THROW_FRMOBJ(Exception, - fmt::format("MocoCasADiSolver failed internally with message: {}", - ex.what())); - } catch (...) { - OPENSIM_THROW_FRMOBJ(Exception, "MocoCasADiSolver failed internally."); - } + // try { + casSolution = casSolver->solve(casGuess); + // } catch(const Exception& ex) { + // OPENSIM_THROW_FRMOBJ(Exception, + // fmt::format("MocoCasADiSolver failed internally with message: {}", + // ex.getMessage())); + // } catch(const casadi::CasadiException& ex) { + // OPENSIM_THROW_FRMOBJ(Exception, + // fmt::format("MocoCasADiSolver failed internally with message: {}", + // ex.what())); + // } catch (...) { + // OPENSIM_THROW_FRMOBJ(Exception, "MocoCasADiSolver failed internally."); + // } OpenSim::Logger::setLevel(origLoggerLevel); MocoSolution mocoSolution = convertToMocoTrajectory( diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 1a3641939c..080f91dd14 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -73,29 +73,30 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, CasOC::Iterate casIt; CasOC::VariablesDM& casVars = casIt.variables; using CasOC::Var; - casVars[Var::initial_time] = mocoIt.getInitialTime(); - casVars[Var::final_time] = mocoIt.getFinalTime(); + // casVars[Var::initial_time] = mocoIt.getInitialTime(); + // casVars[Var::final_time] = mocoIt.getFinalTime(); + casVars[Var::times] = convertToCasADiDMTranspose(mocoIt.getTime()); casVars[Var::states] = convertToCasADiDMTranspose(mocoIt.getStatesTrajectory()); - casadi::DM controls = + casadi::DM controls = convertToCasADiDMTranspose(mocoIt.getControlsTrajectory()); - casadi::DM input_controls = + casadi::DM input_controls = convertToCasADiDMTranspose(mocoIt.getInputControlsTrajectory()); std::vector controlNames = mocoIt.getControlNames(); std::vector inputControlNames = mocoIt.getInputControlNames(); std::vector casControlNames; - int numTotalControls = static_cast(controls.rows()) + + int numTotalControls = static_cast(controls.rows()) + static_cast(input_controls.rows()); casadi::DM casControls(numTotalControls, controls.columns()); - + int ic = 0; int iic = 0; std::sort(inputControlIndexes.begin(), inputControlIndexes.end()); int numInputControls = static_cast(inputControlIndexes.size()); for (int i = 0; i < numTotalControls; ++i) { if (iic < numInputControls && inputControlIndexes[iic] == i) { - casControls(i, casadi::Slice()) = + casControls(i, casadi::Slice()) = input_controls(iic, casadi::Slice()); casControlNames.push_back(inputControlNames[iic]); ++iic; @@ -119,7 +120,6 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, } casVars[Var::parameters] = convertToCasADiDMTranspose(mocoIt.getParameters()); - casIt.times = convertToCasADiDMTranspose(mocoIt.getTime()); casIt.state_names = mocoIt.getStateNames(); casIt.control_names = casControlNames; casIt.multiplier_names = mocoIt.getMultiplierNames(); @@ -155,7 +155,7 @@ inline SimTK::Matrix convertToSimTKMatrix(const casadi::DM& casMatrix) { } template -TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, +TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, std::vector inputControlIndexes = {}) { SimTK::Matrix simtkStates; const auto& casVars = casIt.variables; @@ -172,7 +172,7 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, std::vector controlNames; std::vector inputControlNames; if (numTotalControls) { - SimTK::Matrix allControls = + SimTK::Matrix allControls = convertToSimTKMatrix(casVars.at(Var::controls)); simtkControls.resize(allControls.nrow(), numControls); simtkInputControls.resize(allControls.nrow(), numInputControls); @@ -215,12 +215,12 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, const auto paramsValue = casVars.at(Var::parameters); simtkParameters = convertToSimTKVector(paramsValue); } - SimTK::Vector simtkTimes = convertToSimTKVector(casIt.times); + SimTK::Vector simtkTimes = convertToSimTKVector(casVars.at(Var::times)); - TOut mocoTraj(simtkTimes, casIt.state_names, controlNames, - inputControlNames, casIt.multiplier_names, derivativeNames, - casIt.parameter_names, simtkStates, simtkControls, - simtkInputControls, simtkMultipliers, simtkDerivatives, + TOut mocoTraj(simtkTimes, casIt.state_names, controlNames, + inputControlNames, casIt.multiplier_names, derivativeNames, + casIt.parameter_names, simtkStates, simtkControls, + simtkInputControls, simtkMultipliers, simtkDerivatives, simtkParameters); // Append slack variables. MocoTrajectory requires the slack variables to be @@ -610,8 +610,8 @@ class MocoCasOCProblem : public CasOC::Problem { for (int ic = 0; ic < getNumControls(); ++ic) { simtkControls[ic] = *(controls.ptr() + ic); } - // Updating the Inputs to InputControllers via the - // ControlDistributor does not mark the model controls cache as + // Updating the Inputs to InputControllers via the + // ControlDistributor does not mark the model controls cache as // invalid, so we must do it manually here. model.markControlsAsInvalid(simtkState); } diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index b96e8d128e..db3a322c8f 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -24,14 +24,14 @@ function(SetDefaultCMakeInstallPrefix) get_filename_component(BASE_DIR ${BASE_DIR} DIRECTORY) # Default install prefix for OpenSim dependencies. If user changes # CMAKE_INSTALL_PREFIX, this directory will be removed. - set(DEFAULT_CMAKE_INSTALL_PREFIX + set(DEFAULT_CMAKE_INSTALL_PREFIX ${BASE_DIR}/opensim_dependencies_install CACHE INTERNAL "Default CMAKE_INSTALL_PREFIX for OpenSim dependencies.") if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT) - set(CMAKE_INSTALL_PREFIX + set(CMAKE_INSTALL_PREFIX ${DEFAULT_CMAKE_INSTALL_PREFIX} CACHE PATH @@ -358,12 +358,15 @@ if(SUPERBUILD_casadi) DEFAULT ON DEPENDS ipopt GIT_URL https://github.com/casadi/casadi.git - GIT_TAG 3.6.5 + GIT_TAG 974f0d9d955b1a0b61d892a6b4c107c116aa8600 CMAKE_ARGS -DWITH_IPOPT:BOOL=ON -DIPOPT_LIBRARIES:FILEPATH=ipopt.dll.lib -DIPOPT_INCLUDE_DIRS:PATH=${CMAKE_INSTALL_PREFIX}/ipopt/include/coin-or -DLIB_FULL_ipopt.dll.lib:FILEPATH=${CMAKE_INSTALL_PREFIX}/ipopt/lib/ipopt.dll.lib -DWITH_MUMPS:BOOL=OFF + -DWITH_BUILD_FATROP:BOOL=ON + -DWITH_BUILD_BLASFEO:BOOL=ON + -DWITH_FATROP:BOOL=ON -DWITH_THREAD:BOOL=ON -DWITH_BUILD_MUMPS:BOOL=OFF -DWITH_EXAMPLES:BOOL=OFF @@ -375,10 +378,13 @@ if(SUPERBUILD_casadi) DEFAULT ON DEPENDS ipopt GIT_URL https://github.com/casadi/casadi.git - GIT_TAG 3.6.5 + GIT_TAG 974f0d9d955b1a0b61d892a6b4c107c116aa8600 CMAKE_ARGS -DWITH_IPOPT:BOOL=ON -DWITH_THREAD:BOOL=ON -DWITH_BUILD_MUMPS:BOOL=OFF + -DWITH_BUILD_FATROP:BOOL=ON + -DWITH_BUILD_BLASFEO:BOOL=ON + -DWITH_FATROP:BOOL=ON -DWITH_EXAMPLES:BOOL=OFF -DPKG_CONFIG_USE_CMAKE_PREFIX_PATH:BOOL=ON -DCMAKE_PREFIX_PATH:PATH=${CMAKE_INSTALL_PREFIX}/ipopt From 189caa2b346e365ac7c73e361c3eeb705f75940a Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 29 Jul 2024 10:36:24 -0700 Subject: [PATCH 02/38] Update createTimes() --- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 985b56b341..38202ad27c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -664,9 +664,8 @@ Solution Transcription::solve(const Iterate& guessOrig) { // Resample the guess. // ------------------- - // const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), - // guessOrig.variables.at(final_time)); - auto guess = guessOrig.resample(guessOrig.variables.at(times)); + const auto guessTimes = createTimes(guessOrig.variables.at(times)); + auto guess = guessOrig.resample(guessTimes); // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). @@ -1245,8 +1244,7 @@ Iterate Transcription::createRandomIterateWithinBounds( m_upperBounds.at(kv.first)); } casIterate.variables[times] = createTimes( - casIterate.variables[times](0), - casIterate.variables[times](1)); + casIterate.variables[times]); return casIterate; } From c761f25bc2c7baa0465ea51c9b3dbaa1f7c8819c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 29 Jul 2024 11:09:21 -0700 Subject: [PATCH 03/38] Time constraints now working --- .../Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp | 2 +- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 1 + OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h | 4 ++-- OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp | 3 ++- 4 files changed, 6 insertions(+), 4 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 46730195f2..01c7db352d 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -112,7 +112,7 @@ int main() { // ================== MocoSolution solution = study.solve(); - //solution.write("sliding_mass_solution.sto"); + solution.write("sliding_mass_solution.sto"); // Visualize. // ========== diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 38202ad27c..70711563be 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -100,6 +100,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); m_numConstraints = + m_numGridPoints-2 + // time constraints m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 89d7265be0..c3ce43adf1 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -370,7 +370,7 @@ class Transcription { // Time constraints. for (int i = 0; i < N; ++i) { - if (igrid != 0 && igrid != m_numGridPoints - 1) { + if (igrid + i != 0 && igrid + i != m_numGridPoints - 1) { copyColumn(constraints.times, itime++); } } @@ -481,7 +481,7 @@ class Transcription { // Time constraints. for (int i = 0; i < N; ++i) { - if (igrid != 0 && igrid != m_numGridPoints - 1) { + if (igrid + i != 0 && igrid + i != m_numGridPoints - 1) { copyColumn(out.times, itime++); } } diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 84ef63d0ee..87c914e91f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -195,7 +195,8 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( // Set solver options. // ------------------- Dict solverOptions; - checkPropertyValueIsInSet(getProperty_optim_solver(), {"ipopt", "snopt"}); + checkPropertyValueIsInSet(getProperty_optim_solver(), + {"ipopt", "snopt", "fatrop"}); checkPropertyValueIsInSet(getProperty_transcription_scheme(), {"trapezoidal", "hermite-simpson", "legendre-gauss-1", "legendre-gauss-2", "legendre-gauss-3", "legendre-gauss-4", From 9be0783390ffb179de652d78e71d7795ca684d90 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 29 Jul 2024 12:24:46 -0700 Subject: [PATCH 04/38] Removing commented out lines for time changes --- .../Moco/MocoCasADiSolver/CasOCTranscription.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 70711563be..7b1d35371e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -100,7 +100,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); m_numConstraints = - m_numGridPoints-2 + // time constraints + m_numGridPoints - 2 + // time constraints m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + @@ -123,8 +123,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Create variables. // ----------------- - // m_scaledVars[initial_time] = MX::sym("initial_time"); - // m_scaledVars[final_time] = MX::sym("final_time"); m_scaledVars[times] = MX::sym("times", 1, m_numGridPoints); m_scaledVars[states] = MX::sym("states", m_problem.getNumStates(), m_numGridPoints); @@ -190,18 +188,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, initializeScalingDM(m_shift); initializeScalingDM(m_scale); - // setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); - // setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); - - // setVariableScaling(initial_time, 0, 0, m_problem.getTimeInitialBounds()); - // setVariableScaling(final_time, 0, 0, m_problem.getTimeFinalBounds()); - setVariableBounds(times, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(times, 0, -1, m_problem.getTimeFinalBounds()); setVariableBounds(times, 0, Slice(1, m_numGridPoints - 1), {m_problem.getTimeInitialBounds().lower, m_problem.getTimeFinalBounds().upper}); + // TODO scale initial and final time by their individual bounds? setVariableScaling(times, Slice(), Slice(), {m_problem.getTimeInitialBounds().lower, m_problem.getTimeFinalBounds().upper}); @@ -286,8 +279,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_unscaledVars = unscaleVariables(m_scaledVars); m_duration = m_unscaledVars[times](-1) - m_unscaledVars[times](0); - // m_times = createTimes( - // m_unscaledVars[initial_time], m_unscaledVars[final_time]); m_paramsTrajGrid = MX::repmat(m_unscaledVars[parameters], 1, m_numGridPoints); m_paramsTrajMesh = From d3c56ff9be4892f55ff3f6151e28d899bffcae45 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 2 Aug 2024 11:38:29 -0700 Subject: [PATCH 05/38] Start adding support for symbolically interpolated controls --- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 3 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 55 +++++++++++++------ .../MocoCasADiSolver/CasOCTranscription.h | 3 +- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 11 +++- 4 files changed, 51 insertions(+), 21 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index d6150dea1b..80e83d53ee 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -25,8 +25,6 @@ namespace CasOC { /// This enum describes the different types of optimization variables, and /// are the keys for the Variables map. enum Var { - // initial_time, - // final_time, // Time Variables. times, /// Differential variables. @@ -51,6 +49,7 @@ using Variables = std::unordered_map>; /// Numeric variables for initial guesses and solutions. using VariablesDM = Variables; /// Symbolic variables, used to define the problem. +using VariablesMXVector = Variables; using VariablesMX = Variables; /// This struct is used to obtain initial guesses. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 7b1d35371e..0cdc34347c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -80,7 +80,9 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, int numPointsPerMeshInterval, + const casadi::Matrix& controlPoints, const casadi::DM& pointsForInterpControls) { + // Set the grid. // ------------- // The grid for a transcription scheme includes both mesh points (i.e. @@ -123,22 +125,43 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Create variables. // ----------------- - m_scaledVars[times] = MX::sym("times", 1, m_numGridPoints); - m_scaledVars[states] = - MX::sym("states", m_problem.getNumStates(), m_numGridPoints); - m_scaledVars[controls] = - MX::sym("controls", m_problem.getNumControls(), m_numGridPoints); - m_scaledVars[multipliers] = MX::sym( - "multipliers", m_problem.getNumMultipliers(), m_numGridPoints); - m_scaledVars[derivatives] = MX::sym( - "derivatives", m_problem.getNumDerivatives(), m_numGridPoints); - - // TODO: This assumes that slack variables are applied at all - // collocation points on the mesh interval interior. - m_scaledVars[slacks] = MX::sym( - "slacks", m_problem.getNumSlacks(), m_numMeshInteriorPoints); - m_scaledVars[parameters] = - MX::sym("parameters", m_problem.getNumParameters(), 1); + for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { + m_scaledVectorVars[times].push_back( + MX::sym("time_" + std::to_string(igrid), 1, 1)); + m_scaledVectorVars[states].push_back( + MX::sym("states_" + std::to_string(igrid), + m_problem.getNumStates(), 1)); + m_scaledVectorVars[controls].push_back( + MX::sym("controls_" + std::to_string(igrid), + m_problem.getNumControls(), 1)); + m_scaledVectorVars[multipliers].push_back( + MX::sym("multipliers_" + std::to_string(igrid), + m_problem.getNumMultipliers(), 1)); + m_scaledVectorVars[derivatives].push_back( + MX::sym("derivatives_" + std::to_string(igrid), + m_problem.getNumDerivatives(), 1)); + } + + m_scaledVectorVars[parameters].push_back( + MX::sym("parameters", m_problem.getNumParameters(), 1)); + + // In the Posa et al. 2016 method for enforcing kinematic constraints, + // the mesh interior points will be the mesh interval midpoints in the + // Hermite-Simpson collocation scheme. + for (int imesh = 0; imesh < m_numMeshInteriorPoints; ++imesh) { + m_scaledVectorVars[slacks].push_back( + MX::sym("slacks_" + std::to_string(imesh), + m_problem.getNumSlacks(), 1)); + } + + // Concatenate variables. + m_scaledVars[times] = MX::horzcat(m_scaledVectorVars[times]); + m_scaledVars[states] = MX::horzcat(m_scaledVectorVars[states]); + m_scaledVars[controls] = MX::horzcat(m_scaledVectorVars[controls]); + m_scaledVars[multipliers] = MX::horzcat(m_scaledVectorVars[multipliers]); + m_scaledVars[derivatives] = MX::horzcat(m_scaledVectorVars[derivatives]); + m_scaledVars[slacks] = MX::horzcat(m_scaledVectorVars[slacks]); + m_scaledVars[parameters] = m_scaledVectorVars[parameters][0]; m_meshIndicesMap = createMeshIndices(); std::vector meshIndicesVector; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index c3ce43adf1..3fa1f39fa6 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -74,6 +74,7 @@ class Transcription { void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, int numPointsPerMeshInterval, + const casadi::Matrix& controlPoints, const casadi::DM& pointsForInterpControls = casadi::DM()); /// We assume all functions depend on time and parameters. @@ -154,10 +155,10 @@ class Transcription { int m_numPathConstraintPoints = -1; casadi::DM m_grid; casadi::DM m_pointsForInterpControls; - // casadi::MX m_times; casadi::MX m_duration; private: + VariablesMXVector m_scaledVectorVars; VariablesMX m_scaledVars; VariablesMX m_unscaledVars; casadi::MX m_paramsTrajGrid; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index 3aea1aafb9..4073a55e1c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -34,8 +34,15 @@ class Trapezoidal : public Transcription { OpenSim::Exception, "Enforcing kinematic constraint derivatives " "not supported with trapezoidal transcription."); - createVariablesAndSetBounds(m_solver.getMesh(), - m_problem.getNumStates(), 2); + + const auto& mesh = m_solver.getMesh(); + casadi::Matrix controlPoints; + for (int i = 0; i < static_cast(mesh.size()); ++i) { + controlPoints->push_back(i); + } + + createVariablesAndSetBounds(mesh, + m_problem.getNumStates(), 2, controlPoints); } private: From f89e50aaf25129a5e93c965253649438e1e7619f Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 6 Aug 2024 11:34:02 -0700 Subject: [PATCH 06/38] Define parameters at all time points --- OpenSim/CMakeLists.txt | 2 +- .../Moco/MocoCasADiSolver/CasOCFunction.cpp | 4 +- OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h | 2 +- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 4 +- OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp | 7 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 79 +++++----- .../MocoCasADiSolver/CasOCTranscription.h | 22 ++- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 16 +- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 138 ++++++++++++++++++ 9 files changed, 215 insertions(+), 59 deletions(-) diff --git a/OpenSim/CMakeLists.txt b/OpenSim/CMakeLists.txt index 8d53dedd8f..8ba2fcc16e 100644 --- a/OpenSim/CMakeLists.txt +++ b/OpenSim/CMakeLists.txt @@ -10,6 +10,6 @@ add_subdirectory(Moco) add_subdirectory(Examples) add_subdirectory(Tests) -#add_subdirectory(Sandbox) +add_subdirectory(Sandbox) install(FILES OpenSim.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/OpenSim") diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index 4371833cc6..fcd7f4bbf1 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -217,7 +217,7 @@ casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, } else if (i == 9) { return fullPoint.at(derivatives)(Slice(), -1); } else if (i == 10) { - return fullPoint.at(parameters); + return fullPoint.at(parameters)(Slice(), -1); } else if (i == 11) { // TODO: We should find a way to actually compute the integral // from fullPoint. Or, make the integral an optimization variable. @@ -323,7 +323,7 @@ casadi::DM VelocityCorrection::getSubsetPoint( } else if (i == 2) { return fullPoint.at(slacks)(Slice(), itime); } else if (i == 3) { - return fullPoint.at(parameters); + return fullPoint.at(parameters)(Slice(), itime); } else { return casadi::DM(); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index 07eec8a9b7..7358608ae2 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -98,7 +98,7 @@ class Function : public casadi::Callback { } else if (i == 4) { return fullPoint.at(derivatives)(Slice(), itime); } else if (i == 5) { - return fullPoint.at(parameters); + return fullPoint.at(parameters)(Slice(), itime); } else { return casadi::DM(); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 80e83d53ee..2389700ced 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -25,7 +25,7 @@ namespace CasOC { /// This enum describes the different types of optimization variables, and /// are the keys for the Variables map. enum Var { - // Time Variables. + // Time variables. times, /// Differential variables. states, @@ -36,7 +36,7 @@ enum Var { /// Used for certain methods of solving kinematic constraints. slacks, /// Used in implicit dynamics mode. - derivatives, // TODO: Rename to accelerations? + derivatives, /// Constant in time. parameters, /// For internal use (never actually a key for Variables). diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp index 8927ffb9f8..936294dd7b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp @@ -122,10 +122,9 @@ Solution Solver::solve(const Iterate& guess) const { if (m_sparsity_detection == "initial-guess") { // Interpolate the guess. Iterate guessCopy(guess); - // const auto guessTimes = - // transcription->createTimes(guessCopy.variables.at(initial_time), - // guessCopy.variables.at(final_time)); - guessCopy = guessCopy.resample(guessCopy.variables.at(times)); + const auto guessTimes = + transcription->createTimes(guessCopy.variables.at(times)); + guessCopy = guessCopy.resample(guessTimes); pointsForSparsityDetection->push_back(guessCopy.variables); } else if (m_sparsity_detection == "random") { // Make sure the exact same sparsity pattern is used every time. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 0cdc34347c..587f3c2923 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -82,7 +82,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numPointsPerMeshInterval, const casadi::Matrix& controlPoints, const casadi::DM& pointsForInterpControls) { - + // Set the grid. // ------------- // The grid for a transcription scheme includes both mesh points (i.e. @@ -92,6 +92,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numMeshPoints = (int)m_solver.getMesh().size(); m_numGridPoints = (int)grid.numel(); m_numMeshIntervals = m_numMeshPoints - 1; + m_numTimeConstraints = m_numGridPoints - 2; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; m_numPointsPerMeshInterval = numPointsPerMeshInterval; @@ -102,7 +103,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); m_numConstraints = - m_numGridPoints - 2 + // time constraints + m_numTimeConstraints + + m_problem.getNumParameters() * (m_numGridPoints - 1) + m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + @@ -128,6 +130,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { m_scaledVectorVars[times].push_back( MX::sym("time_" + std::to_string(igrid), 1, 1)); + m_scaledVectorVars[parameters].push_back( + MX::sym("parameters_" + std::to_string(igrid), + m_problem.getNumParameters(), 1)); m_scaledVectorVars[states].push_back( MX::sym("states_" + std::to_string(igrid), m_problem.getNumStates(), 1)); @@ -142,9 +147,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_problem.getNumDerivatives(), 1)); } - m_scaledVectorVars[parameters].push_back( - MX::sym("parameters", m_problem.getNumParameters(), 1)); - // In the Posa et al. 2016 method for enforcing kinematic constraints, // the mesh interior points will be the mesh interval midpoints in the // Hermite-Simpson collocation scheme. @@ -156,12 +158,12 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Concatenate variables. m_scaledVars[times] = MX::horzcat(m_scaledVectorVars[times]); + m_scaledVars[parameters] = MX::horzcat(m_scaledVectorVars[parameters]); m_scaledVars[states] = MX::horzcat(m_scaledVectorVars[states]); m_scaledVars[controls] = MX::horzcat(m_scaledVectorVars[controls]); m_scaledVars[multipliers] = MX::horzcat(m_scaledVectorVars[multipliers]); m_scaledVars[derivatives] = MX::horzcat(m_scaledVectorVars[derivatives]); m_scaledVars[slacks] = MX::horzcat(m_scaledVectorVars[slacks]); - m_scaledVars[parameters] = m_scaledVectorVars[parameters][0]; m_meshIndicesMap = createMeshIndices(); std::vector meshIndicesVector; @@ -183,7 +185,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, std::vector gridIndicesVector(m_numGridPoints); std::iota(gridIndicesVector.begin(), gridIndicesVector.end(), 0); m_gridIndices = makeTimeIndices(gridIndicesVector); - m_meshIndices = makeTimeIndices(meshIndicesVector); m_meshInteriorIndices = makeTimeIndices(meshInteriorIndicesVector); @@ -216,7 +217,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, setVariableBounds(times, 0, Slice(1, m_numGridPoints - 1), {m_problem.getTimeInitialBounds().lower, m_problem.getTimeFinalBounds().upper}); - // TODO scale initial and final time by their individual bounds? setVariableScaling(times, Slice(), Slice(), {m_problem.getTimeInitialBounds().lower, @@ -294,23 +294,14 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, const auto& paramInfos = m_problem.getParameterInfos(); int ip = 0; for (const auto& info : paramInfos) { - setVariableBounds(parameters, ip, 0, info.bounds); - setVariableScaling(parameters, ip, 0, info.bounds); + setVariableBounds(parameters, ip, Slice(), info.bounds); + setVariableScaling(parameters, ip, Slice(), info.bounds); ++ip; } } m_unscaledVars = unscaleVariables(m_scaledVars); m_duration = m_unscaledVars[times](-1) - m_unscaledVars[times](0); - m_paramsTrajGrid = - MX::repmat(m_unscaledVars[parameters], 1, m_numGridPoints); - m_paramsTrajMesh = - MX::repmat(m_unscaledVars[parameters], 1, m_numMeshPoints); - m_paramsTrajMeshInterior = MX::repmat(m_unscaledVars[parameters], 1, - m_numMeshInteriorPoints); - m_paramsTrajPathCon = - MX::repmat(m_unscaledVars[parameters], 1, - m_numPathConstraintPoints); } void Transcription::transcribe() { @@ -332,9 +323,18 @@ void Transcription::transcribe() { // in CasADi? // Initialize memory for time constraints. // --------------------------------------- - m_constraints.times = MX(casadi::Sparsity::dense(1, m_numGridPoints-2)); - m_constraintsLowerBounds.times = DM::zeros(1, m_numGridPoints-2); - m_constraintsUpperBounds.times = DM::zeros(1, m_numGridPoints-2); + m_constraints.times = MX(casadi::Sparsity::dense(1, m_numTimeConstraints)); + m_constraintsLowerBounds.times = DM::zeros(1, m_numTimeConstraints); + m_constraintsUpperBounds.times = DM::zeros(1, m_numTimeConstraints); + + // Initialize memory for parameter constraints. + // -------------------------------------------- + m_constraints.parameters = MX(casadi::Sparsity::dense( + m_problem.getNumParameters(), m_numGridPoints - 1)); + m_constraintsLowerBounds.parameters = + DM::zeros(m_problem.getNumParameters(), m_numGridPoints - 1); + m_constraintsUpperBounds.parameters = + DM::zeros(m_problem.getNumParameters(), m_numGridPoints - 1); // Initialize memory for state derivatives and defects. // ---------------------------------------------------- @@ -379,12 +379,20 @@ void Transcription::transcribe() { // time // ---- - const auto& initialTime = m_unscaledVars[times](0); - const auto& finalTime = m_unscaledVars[times](-1); + const auto& initialTime = m_scaledVars[times](0); + const auto& finalTime = m_scaledVars[times](-1); for (int itime = 1; itime < m_numGridPoints - 1; ++itime) { m_constraints.times(0, itime - 1) = (finalTime - initialTime) * m_grid(itime) + initialTime - - m_unscaledVars[times](itime); + - m_scaledVars[times](itime); + } + + // parameters + // ---------- + for (int itime = 0; itime < m_numGridPoints - 1; ++itime) { + m_constraints.parameters(Slice(), itime) = + m_scaledVars[parameters](Slice(), itime + 1) - + m_scaledVars[parameters](Slice(), itime); } // qdot @@ -588,7 +596,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), - m_unscaledVars[parameters], + m_unscaledVars[parameters](Slice(), -1), integral}, costOut); m_objectiveTerms(iterm++) = casadi::MX::sum1(costOut.at(0)); @@ -662,7 +670,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), - m_unscaledVars[parameters], + m_unscaledVars[parameters](Slice(), -1), integral}, endpointOut); m_constraints.endpoint[iec] = endpointOut.at(0); @@ -1020,7 +1028,8 @@ void Transcription::printConstraintValues(const Iterate& it, printParameterBounds( "Time bounds", time_names, timeValues, timeLower, timeUpper); printParameterBounds("Parameter bounds", it.parameter_names, - vars.at(parameters), lower.at(parameters), upper.at(parameters)); + vars.at(parameters)(0), lower.at(parameters)(0), + upper.at(parameters)(0)); // Constraints. // ============ @@ -1273,7 +1282,6 @@ casadi::MXVector Transcription::evalOnTrajectory( // Assemble input. // Add 1 for time input and 1 for parameters input. MXVector mxIn(inputs.size() + 2); - // mxIn[0] = m_times(timeIndices); mxIn[0] = m_unscaledVars.at(times)(timeIndices); for (int i = 0; i < (int)inputs.size(); ++i) { if (inputs[i] == multibody_states) { @@ -1287,17 +1295,8 @@ casadi::MXVector Transcription::evalOnTrajectory( mxIn[i + 1] = m_unscaledVars.at(inputs[i])(Slice(), timeIndices); } } - if (&timeIndices == &m_gridIndices) { - mxIn[mxIn.size() - 1] = m_paramsTrajGrid; - } else if (&timeIndices == &m_meshIndices) { - mxIn[mxIn.size() - 1] = m_paramsTrajMesh; - } else if (&timeIndices == &m_meshInteriorIndices) { - mxIn[mxIn.size() - 1] = m_paramsTrajMeshInterior; - } else if (&timeIndices == &m_pathConstraintIndices) { - mxIn[mxIn.size() - 1] = m_paramsTrajPathCon; - } else { - OPENSIM_THROW(OpenSim::Exception, "Internal error."); - } + mxIn[mxIn.size() - 1] = m_unscaledVars.at(parameters)(Slice(), timeIndices); + MXVector mxOut; trajFunc.call(mxIn, mxOut); return mxOut; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 3fa1f39fa6..a7b9cac66e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -126,6 +126,7 @@ class Transcription { template struct Constraints { T times; + T parameters; T defects; T multibody_residuals; T auxiliary_residuals; @@ -151,6 +152,7 @@ class Transcription { int m_numPointsPerMeshInterval = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; + int m_numTimeConstraints = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; casadi::DM m_grid; @@ -161,10 +163,10 @@ class Transcription { VariablesMXVector m_scaledVectorVars; VariablesMX m_scaledVars; VariablesMX m_unscaledVars; - casadi::MX m_paramsTrajGrid; - casadi::MX m_paramsTrajMesh; - casadi::MX m_paramsTrajMeshInterior; - casadi::MX m_paramsTrajPathCon; + // casadi::MX m_paramsTrajGrid; + // casadi::MX m_paramsTrajMesh; + // casadi::MX m_paramsTrajMeshInterior; + // casadi::MX m_paramsTrajPathCon; VariablesDM m_lowerBounds; VariablesDM m_upperBounds; VariablesDM m_shift; @@ -366,6 +368,7 @@ class Transcription { int N = m_numPointsPerMeshInterval - 1; int icon = 0; int itime = 0; + int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; @@ -376,6 +379,11 @@ class Transcription { } } + // Parameter constraints. + for (int i = 0; i < N; ++i) { + copyColumn(constraints.parameters, iparam++); + } + // Defect constraints. copyColumn(constraints.defects, imesh); @@ -477,6 +485,7 @@ class Transcription { int N = m_numPointsPerMeshInterval - 1; int icon = 0; int itime = 0; + int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; @@ -487,6 +496,11 @@ class Transcription { } } + // Parameter constraints. + for (int i = 0; i < N; ++i) { + copyColumn(out.parameters, iparam++); + } + // Defect constraints. copyColumn(out.defects, imesh); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 080f91dd14..a8bb5abf80 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -73,8 +73,6 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, CasOC::Iterate casIt; CasOC::VariablesDM& casVars = casIt.variables; using CasOC::Var; - // casVars[Var::initial_time] = mocoIt.getInitialTime(); - // casVars[Var::final_time] = mocoIt.getFinalTime(); casVars[Var::times] = convertToCasADiDMTranspose(mocoIt.getTime()); casVars[Var::states] = convertToCasADiDMTranspose(mocoIt.getStatesTrajectory()); @@ -118,8 +116,15 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, casVars[Var::derivatives] = convertToCasADiDMTranspose(mocoIt.getDerivativesTrajectory()); } - casVars[Var::parameters] = - convertToCasADiDMTranspose(mocoIt.getParameters()); + + const SimTK::RowVector& parameters = mocoIt.getParameters(); + SimTK::Matrix parametersTrajectory(mocoIt.getNumTimes(), + mocoIt.getNumParameters()); + for (int i = 0; i < mocoIt.getNumTimes(); ++i) { + parametersTrajectory.updRow(i) = parameters; + } + casVars[Var::parameters] = convertToCasADiDMTranspose(parametersTrajectory); + casIt.state_names = mocoIt.getStateNames(); casIt.control_names = casControlNames; casIt.multiplier_names = mocoIt.getMultiplierNames(); @@ -212,7 +217,8 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, } SimTK::RowVector simtkParameters; if (!casIt.parameter_names.empty()) { - const auto paramsValue = casVars.at(Var::parameters); + const auto paramsValue = + casVars.at(Var::parameters)(casadi::Slice(), 0); simtkParameters = convertToSimTKVector(paramsValue); } SimTK::Vector simtkTimes = convertToSimTKVector(casVars.at(Var::times)); diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 8e30852df9..ae4865f926 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -20,10 +20,148 @@ // code during development. #include +#include using namespace OpenSim; +const double STIFFNESS = 100.0; // N/m +const double MASS = 5.0; // kg +const double FINAL_TIME = SimTK::Pi * sqrt(MASS / STIFFNESS); +std::unique_ptr createOscillatorModel() { + auto model = make_unique(); + model->setName("oscillator"); + model->set_gravity(SimTK::Vec3(0, 0, 0)); + // We will optimize the mass of this body in the test below. Here, we'll set + // the model with "incorrect" mass value, which the test will use as the + // initial guess. + auto* body = new Body("body", 0.5*MASS, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + + // Allows translation along x. + auto* joint = new SliderJoint("slider", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* spring = new SpringGeneralizedForce(); + spring->set_coordinate("position"); + spring->setRestLength(0.0); + spring->setStiffness(STIFFNESS); + spring->setViscosity(0.0); + model->addComponent(spring); + + return model; +} + +class FinalPositionGoal : public MocoGoal { +OpenSim_DECLARE_CONCRETE_OBJECT(FinalPositionGoal, MocoGoal); +protected: + void initializeOnModelImpl(const Model&) const override { + setRequirements(0, 1); + } + void calcGoalImpl(const GoalInput& input, + SimTK::Vector& cost) const override { + const auto& finalPosition = input.final_state.getY()[0]; + + cost[0] = (finalPosition - 0.5) * (finalPosition - 0.5); + } +}; + +/// Optimize the mass of a simple harmonic oscillator such that it follows the +/// correct trajectory specified by the state bounds and the FinalPositionCost. +/// This tests the ability for MocoParameter to optimize a simple scalar model +/// property value. +void testOscillatorMass() { + int N = 25; + + MocoStudy study; + study.setName("oscillator_mass"); + MocoProblem& mp = study.updProblem(); + mp.setModel(createOscillatorModel()); + mp.setTimeBounds(0, FINAL_TIME); + mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + mp.addParameter("oscillator_mass", "body", "mass", MocoBounds(0, 10)); + + mp.addGoal(); + + auto& ms = study.initSolver(); + ms.set_num_mesh_intervals(N); + + MocoSolution sol = study.solve(); + //sol.write("testMocoParameters_testOscillatorMass_sol.sto"); + + // CHECK(sol.getParameter("oscillator_mass") == + // Catch::Approx(MASS).epsilon(0.003)); +} + +std::unique_ptr createOscillatorTwoSpringsModel() { + auto model = make_unique(); + model->setName("oscillator_two_springs"); + model->set_gravity(SimTK::Vec3(0, 0, 0)); + auto* body = new Body("body", MASS, SimTK::Vec3(0), SimTK::Inertia(0)); + model->addComponent(body); + + // Allows translation along x. + auto* joint = new SliderJoint("slider", model->getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("position"); + model->addComponent(joint); + + auto* spring1 = new SpringGeneralizedForce(); + spring1->setName("spring1"); + spring1->set_coordinate("position"); + spring1->setRestLength(0.0); + spring1->setStiffness(0.25*STIFFNESS); + spring1->setViscosity(0.0); + model->addComponent(spring1); + + auto* spring2 = new SpringGeneralizedForce(); + spring2->setName("spring2"); + spring2->set_coordinate("position"); + spring2->setRestLength(0.0); + spring2->setStiffness(0.25*STIFFNESS); + spring2->setViscosity(0.0); + model->addComponent(spring2); + + return model; +} + +void testSpringParameter() { + int N = 25; + + MocoStudy study; + study.setName("oscillator_spring_stiffnesses"); + MocoProblem& mp = study.updProblem(); + mp.setModel(createOscillatorTwoSpringsModel()); + mp.setTimeBounds(0, FINAL_TIME); + mp.setStateInfo("/slider/position/value", {-5.0, 5.0}, -0.5, {0.25, 0.75}); + mp.setStateInfo("/slider/position/speed", {-20, 20}, 0, 0); + + // Optimize a single stiffness value and apply to both springs. + std::vector components = {"spring1", "spring2"}; + mp.addParameter("spring_stiffness", components, "stiffness", + MocoBounds(0, 100)); + + mp.addGoal(); + + auto& ms = study.initSolver(); + ms.set_num_mesh_intervals(N); + ms.set_parameters_require_initsystem(false); + + MocoSolution sol = study.solve(); + //sol.write("testMocoParameters_testOscillatorMassTwoSprings_sol.sto"); + + // Since springs add in parallel, both stiffness must be the same value + // and equal half the original spring stiffness. + // CHECK(sol.getParameter("spring_stiffness") == + // Catch::Approx(0.5*STIFFNESS).epsilon(0.003)); +} + int main() { + testSpringParameter(); + return EXIT_SUCCESS; } From ec111ed8f7ea417b03a75f543a2d0565f19dda23 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 6 Aug 2024 16:10:09 -0700 Subject: [PATCH 07/38] Testing with current control interpolation --- .../exampleSlidingMass/exampleSlidingMass.cpp | 6 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 16 ++++ .../MocoCasADiSolver/CasOCHermiteSimpson.h | 4 + .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 16 ++++ .../MocoCasADiSolver/CasOCLegendreGauss.h | 8 +- .../CasOCLegendreGaussRadau.cpp | 17 ++++ .../CasOCLegendreGaussRadau.h | 8 ++ .../MocoCasADiSolver/CasOCTranscription.cpp | 36 ++++++-- .../MocoCasADiSolver/CasOCTranscription.h | 85 +++++++++++++++---- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 11 ++- 10 files changed, 177 insertions(+), 30 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 01c7db352d..0815f36613 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -102,8 +102,10 @@ int main() { // Configure the solver. // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); - solver.set_num_mesh_intervals(50); + solver.set_num_mesh_intervals(5); solver.set_parallel(0); + solver.set_optim_solver("fatrop"); + solver.set_transcription_scheme("legendre-gauss-1"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); @@ -116,7 +118,7 @@ int main() { // Visualize. // ========== - study.visualize(solution); + // study.visualize(solution); return EXIT_SUCCESS; } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 75dc44089f..4e2f2a6f34 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -105,4 +105,20 @@ void HermiteSimpson::calcInterpolatingControlsImpl( } } +// void HermiteSimpson::calcExtrapolatedControlsImpl(casadi::MX& controls) const { +// if (m_problem.getNumControls()) { +// int time_i; +// int time_mid; +// int time_ip1; +// for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { +// time_i = 2 * imesh; +// time_mid = 2 * imesh + 1; +// time_ip1 = 2 * imesh + 2; +// const auto c_i = controls(Slice(), time_i); +// const auto c_ip1 = controls(Slice(), time_ip1); +// controls(Slice(), time_mid) = 0.5 * (c_ip1 + c_i); +// } +// } +// } + } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index c018be51b4..9d105a2616 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -53,11 +53,14 @@ class HermiteSimpson : public Transcription { pointsForInterpControls = casadi::DM::zeros(1, m_solver.getMesh().size() - 1); } + // std::vector controlPoints; for (int i = 0; i < grid.numel(); ++i) { if (i % 2 == 0) { grid(i) = mesh[i / 2]; + // controlPoints.push_back(true); } else { grid(i) = .5 * (mesh[i / 2] + mesh[i / 2 + 1]); + // controlPoints.push_back(false); if (interpControls) { pointsForInterpControls(i / 2) = grid(i); } @@ -74,6 +77,7 @@ class HermiteSimpson : public Transcription { const casadi::MX& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; + // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 2a7415abdb..8ced6ca5f5 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -97,4 +97,20 @@ void LegendreGauss::calcInterpolatingControlsImpl( } } +// void LegendreGauss::calcExtrapolatedControlsImpl(casadi::MX& controls) const { +// if (m_problem.getNumControls()) { +// // TODO +// // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { +// // const int igrid = imesh * (m_degree + 1); +// // const auto c_i = controls(Slice(), igrid); +// // const auto c_ip1 = controls(Slice(), igrid + m_degree + 1); +// // for (int d = 0; d < m_degree; ++d) { +// // const auto c_t = controls(Slice(), igrid + d + 1); +// // interpControls(Slice(), imesh * m_degree + d) = +// // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); +// // } +// // } +// } +// } + } // namespace CasOC \ No newline at end of file diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 62e60508fd..e4a3413e1a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -89,13 +89,16 @@ class LegendreGauss : public Transcription { m_quadratureCoefficients); // Create the grid points. + // std::vector controlPoints; for (int imesh = 0; imesh < numMeshIntervals; ++imesh) { const double t_i = mesh[imesh]; const double t_ip1 = mesh[imesh + 1]; int igrid = imesh * (m_degree + 1); grid(igrid) = t_i; + // controlPoints.push_back(false); for (int d = 0; d < m_degree; ++d) { grid(igrid + d + 1) = t_i + (t_ip1 - t_i) * m_legendreRoots[d]; + // controlPoints.push_back(true); if (interpControls) { pointsForInterpControls(imesh * m_degree + d) = grid(igrid + d + 1); @@ -103,9 +106,9 @@ class LegendreGauss : public Transcription { } } grid(numGridPoints - 1) = mesh[numMeshIntervals]; + // controlPoints.push_back(false); createVariablesAndSetBounds(grid, - (m_degree + 1) * m_problem.getNumStates(), - m_degree + 2, + (m_degree + 1) * m_problem.getNumStates(), m_degree + 2, pointsForInterpControls); } @@ -116,6 +119,7 @@ class LegendreGauss : public Transcription { const casadi::MX& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; + // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; int m_degree; std::vector m_legendreRoots; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index bbee41839a..38d6cd83c2 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -90,4 +90,21 @@ void LegendreGaussRadau::calcInterpolatingControlsImpl( } } +// void LegendreGaussRadau::calcExtrapolatedControlsImpl( +// casadi::MX& controls) const { +// if (m_problem.getNumControls()) { +// // TODO +// // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { +// // const int igrid = imesh * m_degree; +// // const auto c_i = controls(Slice(), igrid); +// // const auto c_ip1 = controls(Slice(), igrid + m_degree); +// // for (int d = 0; d < m_degree-1; ++d) { +// // const auto c_t = controls(Slice(), igrid + d + 1); +// // interpControls(Slice(), imesh * (m_degree - 1) + d) = +// // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); +// // } +// // } +// } +// } + } // namespace CasOC \ No newline at end of file diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 4cd7ccc8cd..cd4fa7bb8e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -105,6 +105,13 @@ class LegendreGaussRadau : public Transcription { } } grid(numGridPoints - 1) = mesh[numMeshIntervals]; + + // std::vector controlPoints; + // controlPoints.push_back(false); + // for (int i = 0; i < numGridPoints - 1; ++i) { + // controlPoints.push_back(true); + // } + createVariablesAndSetBounds(grid, m_degree * m_problem.getNumStates(), m_degree + 1, @@ -118,6 +125,7 @@ class LegendreGaussRadau : public Transcription { const casadi::MX& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; + // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; int m_degree; std::vector m_legendreRoots; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 587f3c2923..355a9739cf 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -80,8 +80,8 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, int numPointsPerMeshInterval, - const casadi::Matrix& controlPoints, const casadi::DM& pointsForInterpControls) { + // const std::vector& controlPoints) { // Set the grid. // ------------- @@ -97,6 +97,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; m_numPointsPerMeshInterval = numPointsPerMeshInterval; m_pointsForInterpControls = pointsForInterpControls; + // m_controlPoints = controlPoints; m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() : 0; @@ -127,6 +128,11 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Create variables. // ----------------- + // OPENSIM_THROW_IF( + // static_cast(m_controlPoints.size()) != m_numGridPoints, + // OpenSim::Exception, + // "The number of control points must match the number of grid " + // "points."); for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { m_scaledVectorVars[times].push_back( MX::sym("time_" + std::to_string(igrid), 1, 1)); @@ -139,6 +145,15 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVectorVars[controls].push_back( MX::sym("controls_" + std::to_string(igrid), m_problem.getNumControls(), 1)); + // if (m_controlPoints[igrid]) { + // m_scaledVectorVars[controls].push_back( + // MX::sym("controls_" + std::to_string(igrid), + // m_problem.getNumControls(), 1)); + // } else { + // m_scaledVectorVars[controls].push_back( + // MX(m_problem.getNumControls(), 1)); + // } + // TODO should multipliers and derivatives also follow controlPoints? m_scaledVectorVars[multipliers].push_back( MX::sym("multipliers_" + std::to_string(igrid), m_problem.getNumMultipliers(), 1)); @@ -302,6 +317,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_unscaledVars = unscaleVariables(m_scaledVars); m_duration = m_unscaledVars[times](-1) - m_unscaledVars[times](0); + + // calcExtrapolatedControls(); } void Transcription::transcribe() { @@ -729,7 +746,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { options[m_solver.getOptimSolver()] = m_solver.getSolverOptions(); } - auto x = flattenVariables(m_scaledVars); + auto x = flattenVariablesMX(m_scaledVectorVars); casadi_int numVariables = x.numel(); // The m_constraints symbolic vector holds all of the expressions for @@ -769,6 +786,15 @@ Solution Transcription::solve(const Iterate& guessOrig) { jacobian.sparsity().to_file( prefix + "constraint_Jacobian_sparsity.mtx"); } + if (m_solver.getOptimSolver() == "fatrop") { + options["structure_detection"] = "auto"; + std::vector equality; + for (int i = 0; i < 7*m_numMeshIntervals - 1; ++i) { + equality.push_back(true); + } + options["equality"] = equality; + options["debug"] = true; + } const casadi::Function nlpFunc = casadi::nlpsol("nlp", m_solver.getOptimSolver(), nlp, options); @@ -776,9 +802,9 @@ Solution Transcription::solve(const Iterate& guessOrig) { // -------------------------------------------------------- // The inputs and outputs of nlpFunc are numeric (casadi::DM). const casadi::DMDict nlpResult = nlpFunc(casadi::DMDict{ - {"x0", flattenVariables(scaleVariables(guess.variables))}, - {"lbx", flattenVariables(scaleVariables(m_lowerBounds))}, - {"ubx", flattenVariables(scaleVariables(m_upperBounds))}, + {"x0", flattenVariablesDM(scaleVariables(guess.variables))}, + {"lbx", flattenVariablesDM(scaleVariables(m_lowerBounds))}, + {"ubx", flattenVariablesDM(scaleVariables(m_upperBounds))}, {"lbg", flattenConstraints(m_constraintsLowerBounds)}, {"ubg", flattenConstraints(m_constraintsUpperBounds)}}); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index a7b9cac66e..caae97d260 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -69,13 +69,12 @@ class Transcription { /// overridden virtual methods are accessible to the base class. This /// implementation allows initialization to occur during construction, /// avoiding an extra call on the instantiated object. - /// pointsForInterpControls are grid points at which the transcription - /// scheme applies constraints between control points. + /// TODO control points void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, int numPointsPerMeshInterval, - const casadi::Matrix& controlPoints, const casadi::DM& pointsForInterpControls = casadi::DM()); + // const std::vector& controlPoints); /// We assume all functions depend on time and parameters. /// "inputs" is prepended by time and postpended (?) by parameters. @@ -158,15 +157,12 @@ class Transcription { casadi::DM m_grid; casadi::DM m_pointsForInterpControls; casadi::MX m_duration; + // std::vector m_controlPoints; private: VariablesMXVector m_scaledVectorVars; VariablesMX m_scaledVars; VariablesMX m_unscaledVars; - // casadi::MX m_paramsTrajGrid; - // casadi::MX m_paramsTrajMesh; - // casadi::MX m_paramsTrajMeshInterior; - // casadi::MX m_paramsTrajPathCon; VariablesDM m_lowerBounds; VariablesDM m_upperBounds; VariablesDM m_shift; @@ -206,6 +202,13 @@ class Transcription { OPENSIM_THROW_IF(m_pointsForInterpControls.numel(), OpenSim::Exception, "Must provide constraints for interpolating controls.") } + // virtual void calcExtrapolatedControlsImpl(casadi::MX& /*controls*/) const { + // bool hasExtrapolatedControls = std::any_of( + // m_controlPoints.begin(), m_controlPoints.end(), + // [](bool b) { return !b; }); + // OPENSIM_THROW_IF(hasExtrapolatedControls, OpenSim::Exception, + // "Must provide scheme for extrapolated controls.") + // } void transcribe(); void setObjectiveAndEndpointConstraints(); @@ -217,25 +220,77 @@ class Transcription { calcInterpolatingControlsImpl( m_unscaledVars.at(controls), m_constraints.interp_controls); } + // void calcExtrapolatedControls() { + // calcExtrapolatedControlsImpl(m_scaledVars.at(controls)); + // } /// Use this function to ensure you iterate through variables in the same /// order. template static std::vector getSortedVarKeys(const Variables& vars) { std::vector keys; - for (const auto& kv : vars) { keys.push_back(kv.first); } - std::sort(keys.begin(), keys.end()); + keys.push_back(times); + keys.push_back(parameters); + keys.push_back(states); + keys.push_back(controls); + keys.push_back(multipliers); + keys.push_back(derivatives); + // for (const auto& kv : vars) { keys.push_back(kv.first); } + // std::sort(keys.begin(), keys.end()); return keys; } /// Convert the map of variables into a column vector, for passing onto /// nlpsol(), etc. - template - static T flattenVariables(const CasOC::Variables& vars) { - std::vector stdvec; - for (const auto& key : getSortedVarKeys(vars)) { - stdvec.push_back(vars.at(key)); + // template + // static T flattenVariables(const CasOC::Variables& vars) { + // std::vector stdvec; + // for (const auto& key : getSortedVarKeys(vars)) { + // stdvec.push_back(vars.at(key)); + // } + // return T::veccat(stdvec); + // } + casadi::MX flattenVariablesMX(const CasOC::VariablesMXVector& vars) const { + std::vector stdvec; + int N = m_numPointsPerMeshInterval - 1; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(times)[igrid + i]); + } + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(states)[igrid + i]); + } + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(controls)[igrid + i]); + } + } + stdvec.push_back(vars.at(times)[m_numGridPoints - 1]); + stdvec.push_back(vars.at(states)[m_numGridPoints - 1]); + stdvec.push_back(vars.at(controls)[m_numGridPoints - 1]); + + return casadi::MX::veccat(stdvec); + } + + casadi::DM flattenVariablesDM(const CasOC::VariablesDM& vars) const { + std::vector stdvec; + int N = m_numPointsPerMeshInterval - 1; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(times)(casadi::Slice(), igrid + i)); + } + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(states)(casadi::Slice(), igrid + i)); + } + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(controls)(casadi::Slice(), igrid + i)); + } } - return T::veccat(stdvec); + stdvec.push_back(vars.at(times)(casadi::Slice(), m_numGridPoints - 1)); + stdvec.push_back(vars.at(states)(casadi::Slice(), m_numGridPoints - 1)); + stdvec.push_back(vars.at(controls)(casadi::Slice(), m_numGridPoints - 1)); + + return casadi::DM::veccat(stdvec); } /// Convert the 'x' column vector into separate variables. CasOC::VariablesDM expandVariables(const casadi::DM& x) const { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index 4073a55e1c..d47bbbe6d4 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -36,13 +36,12 @@ class Trapezoidal : public Transcription { "not supported with trapezoidal transcription."); const auto& mesh = m_solver.getMesh(); - casadi::Matrix controlPoints; - for (int i = 0; i < static_cast(mesh.size()); ++i) { - controlPoints->push_back(i); - } - + // std::vector controlPoints; + // for (int i = 0; i < static_cast(mesh.size()); ++i) { + // controlPoints.push_back(true); + // } createVariablesAndSetBounds(mesh, - m_problem.getNumStates(), 2, controlPoints); + m_problem.getNumStates(), 2); } private: From 0d3946ce79e5bbdd5193a7b92d2b6c391bed4e15 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 6 Aug 2024 17:12:24 -0700 Subject: [PATCH 08/38] Treat time variables as parameters --- .../exampleSlidingMass/exampleSlidingMass.cpp | 6 +- .../Moco/MocoCasADiSolver/CasOCFunction.cpp | 6 +- OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h | 2 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 7 +- .../MocoCasADiSolver/CasOCHermiteSimpson.h | 4 +- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 4 +- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 7 +- .../MocoCasADiSolver/CasOCLegendreGauss.h | 4 +- .../CasOCLegendreGaussRadau.cpp | 7 +- .../CasOCLegendreGaussRadau.h | 4 +- OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp | 5 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 124 ++++++++++-------- .../MocoCasADiSolver/CasOCTranscription.h | 60 ++++++--- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 4 +- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 2 +- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 19 ++- 16 files changed, 159 insertions(+), 106 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 0815f36613..ef9808f623 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -102,10 +102,10 @@ int main() { // Configure the solver. // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); - solver.set_num_mesh_intervals(5); + solver.set_num_mesh_intervals(50); solver.set_parallel(0); - solver.set_optim_solver("fatrop"); - solver.set_transcription_scheme("legendre-gauss-1"); + solver.set_optim_solver("ipopt"); + // solver.set_transcription_scheme("legendre-gauss-1"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index fcd7f4bbf1..c75c03830e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -197,7 +197,7 @@ casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, casadi_int i) const { using casadi::Slice; if (i == 0) { - return fullPoint.at(times)(0); + return fullPoint.at(initial_time)(0); } else if (i == 1) { return fullPoint.at(states)(Slice(), 0); } else if (i == 2) { @@ -207,7 +207,7 @@ casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, } else if (i == 4) { return fullPoint.at(derivatives)(Slice(), 0); } else if (i == 5) { - return fullPoint.at(times)(-1); + return fullPoint.at(final_time)(0); } else if (i == 6) { return fullPoint.at(states)(Slice(), -1); } else if (i == 7) { @@ -317,7 +317,7 @@ casadi::DM VelocityCorrection::getSubsetPoint( const int NMBS = m_casProblem->getNumStates() - m_casProblem->getNumAuxiliaryStates(); if (i == 0) { - return fullPoint.at(times)(0); + return fullPoint.at(initial_time)(0); } else if (i == 1) { return fullPoint.at(states)(Slice(0, NMBS), itime); } else if (i == 2) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index 7358608ae2..386d2301df 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -88,7 +88,7 @@ class Function : public casadi::Callback { int itime = 0; using casadi::Slice; if (i == 0) { - return fullPoint.at(times)(0); + return fullPoint.at(initial_time)(0); } else if (i == 1) { return fullPoint.at(states)(Slice(), itime); } else if (i == 2) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 4e2f2a6f34..5d74f89de7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -50,9 +50,8 @@ DM HermiteSimpson::createMeshIndicesImpl() const { return indices; } -void HermiteSimpson::calcDefectsImpl(const casadi::MX& times, - const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const { +void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); @@ -68,7 +67,7 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& times, time_mid = 2 * imesh + 1; time_ip1 = 2 * imesh + 2; - const auto h = times(time_ip1) - times(time_i); + const auto h = m_times(time_ip1) - m_times(time_i); const auto x_i = x(Slice(), time_i); const auto x_mid = x(Slice(), time_mid); const auto x_ip1 = x(Slice(), time_ip1); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 9d105a2616..567fc5db04 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -73,8 +73,8 @@ class HermiteSimpson : public Transcription { private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 2389700ced..0997108b0e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -26,7 +26,8 @@ namespace CasOC { /// are the keys for the Variables map. enum Var { // Time variables. - times, + initial_time, + final_time, /// Differential variables. states, /// Algebraic variables. @@ -55,6 +56,7 @@ using VariablesMX = Variables; /// This struct is used to obtain initial guesses. struct Iterate { VariablesDM variables; + casadi::DM times; std::vector state_names; std::vector control_names; std::vector multiplier_names; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 8ced6ca5f5..3129c5935e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -54,15 +54,14 @@ DM LegendreGauss::createMeshIndicesImpl() const { return indices; } -void LegendreGauss::calcDefectsImpl(const casadi::MX& times, - const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const { +void LegendreGauss::calcDefectsImpl(const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * (m_degree + 1); - const auto h = times(igrid + m_degree + 1) - times(igrid); + const auto h = m_times(igrid + m_degree + 1) - m_times(igrid); const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); const auto xdot_i = xdot(Slice(), Slice(igrid + 1, igrid + m_degree + 1)); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index e4a3413e1a..0738f660b0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -115,8 +115,8 @@ class LegendreGauss : public Transcription { private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 38d6cd83c2..4713138e64 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -52,15 +52,14 @@ DM LegendreGaussRadau::createMeshIndicesImpl() const { return indices; } -void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& times, - const casadi::MX& x, const casadi::MX& xdot, - casadi::MX& defects) const { +void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, + const casadi::MX& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; - const auto h = times(igrid + m_degree) - times(igrid); + const auto h = m_times(igrid + m_degree) - m_times(igrid); const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); const auto xdot_i = xdot(Slice(), Slice(igrid + 1, igrid + m_degree + 1)); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index cd4fa7bb8e..4413adce6b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -121,8 +121,8 @@ class LegendreGaussRadau : public Transcription { private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp index 936294dd7b..1054bc854e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp @@ -122,8 +122,9 @@ Solution Solver::solve(const Iterate& guess) const { if (m_sparsity_detection == "initial-guess") { // Interpolate the guess. Iterate guessCopy(guess); - const auto guessTimes = - transcription->createTimes(guessCopy.variables.at(times)); + const auto guessTimes = transcription->createTimes( + guessCopy.variables.at(initial_time), + guessCopy.variables.at(final_time)); guessCopy = guessCopy.resample(guessTimes); pointsForSparsityDetection->push_back(guessCopy.variables); } else if (m_sparsity_detection == "random") { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 355a9739cf..e10034ad99 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -61,6 +61,9 @@ class NlpsolCallback : public casadi::Callback { Iterate iterate = m_problem.createIterate(); iterate.variables = m_transcription.expandVariables(args.at(0)); iterate.iteration = evalCount; + iterate.times = + m_transcription.createTimes(iterate.variables[initial_time], + iterate.variables[final_time]); m_problem.intermediateCallbackWithIterate(iterate); } m_problem.intermediateCallback(); @@ -92,7 +95,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numMeshPoints = (int)m_solver.getMesh().size(); m_numGridPoints = (int)grid.numel(); m_numMeshIntervals = m_numMeshPoints - 1; - m_numTimeConstraints = m_numGridPoints - 2; + m_numParameterConstraints = m_numGridPoints - 1; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; m_numPointsPerMeshInterval = numPointsPerMeshInterval; @@ -104,8 +107,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); m_numConstraints = - m_numTimeConstraints + - m_problem.getNumParameters() * (m_numGridPoints - 1) + + (m_problem.getNumParameters() + 2) * m_numParameterConstraints + m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + @@ -134,8 +136,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // "The number of control points must match the number of grid " // "points."); for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { - m_scaledVectorVars[times].push_back( - MX::sym("time_" + std::to_string(igrid), 1, 1)); + m_scaledVectorVars[initial_time].push_back( + MX::sym("initial_time_" + std::to_string(igrid), 1, 1)); + m_scaledVectorVars[final_time].push_back( + MX::sym("final_time_" + std::to_string(igrid), 1, 1)); m_scaledVectorVars[parameters].push_back( MX::sym("parameters_" + std::to_string(igrid), m_problem.getNumParameters(), 1)); @@ -172,7 +176,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } // Concatenate variables. - m_scaledVars[times] = MX::horzcat(m_scaledVectorVars[times]); + m_scaledVars[initial_time] = MX::horzcat(m_scaledVectorVars[initial_time]); + m_scaledVars[final_time] = MX::horzcat(m_scaledVectorVars[final_time]); m_scaledVars[parameters] = MX::horzcat(m_scaledVectorVars[parameters]); m_scaledVars[states] = MX::horzcat(m_scaledVectorVars[states]); m_scaledVars[controls] = MX::horzcat(m_scaledVectorVars[controls]); @@ -227,15 +232,16 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, initializeScalingDM(m_shift); initializeScalingDM(m_scale); - setVariableBounds(times, 0, 0, m_problem.getTimeInitialBounds()); - setVariableBounds(times, 0, -1, m_problem.getTimeFinalBounds()); - setVariableBounds(times, 0, Slice(1, m_numGridPoints - 1), - {m_problem.getTimeInitialBounds().lower, - m_problem.getTimeFinalBounds().upper}); - // TODO scale initial and final time by their individual bounds? - setVariableScaling(times, Slice(), Slice(), - {m_problem.getTimeInitialBounds().lower, - m_problem.getTimeFinalBounds().upper}); + setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); + setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); + // TODO use inf bounds + setVariableBounds(initial_time, 0, Slice(1, m_numGridPoints), {-100, 100}); + setVariableBounds(final_time, 0, Slice(1, m_numGridPoints), {-100, 100}); + + setVariableScaling(initial_time, 0, Slice(), + m_problem.getTimeInitialBounds()); + setVariableScaling(final_time, 0, Slice(), + m_problem.getTimeFinalBounds()); { const auto& stateInfos = m_problem.getStateInfos(); @@ -315,8 +321,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } m_unscaledVars = unscaleVariables(m_scaledVars); - - m_duration = m_unscaledVars[times](-1) - m_unscaledVars[times](0); + m_times = createTimes( + m_unscaledVars[initial_time], m_unscaledVars[final_time]); + m_duration = m_unscaledVars[final_time](0) - + m_unscaledVars[initial_time](0); // calcExtrapolatedControls(); } @@ -340,9 +348,19 @@ void Transcription::transcribe() { // in CasADi? // Initialize memory for time constraints. // --------------------------------------- - m_constraints.times = MX(casadi::Sparsity::dense(1, m_numTimeConstraints)); - m_constraintsLowerBounds.times = DM::zeros(1, m_numTimeConstraints); - m_constraintsUpperBounds.times = DM::zeros(1, m_numTimeConstraints); + m_constraints.initial_time = + MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); + m_constraintsLowerBounds.initial_time = + DM::zeros(1, m_numParameterConstraints); + m_constraintsUpperBounds.initial_time = + DM::zeros(1, m_numParameterConstraints); + + m_constraints.final_time = + MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); + m_constraintsLowerBounds.final_time = + DM::zeros(1, m_numParameterConstraints); + m_constraintsUpperBounds.final_time = + DM::zeros(1, m_numParameterConstraints); // Initialize memory for parameter constraints. // -------------------------------------------- @@ -396,12 +414,13 @@ void Transcription::transcribe() { // time // ---- - const auto& initialTime = m_scaledVars[times](0); - const auto& finalTime = m_scaledVars[times](-1); - for (int itime = 1; itime < m_numGridPoints - 1; ++itime) { - m_constraints.times(0, itime - 1) = - (finalTime - initialTime) * m_grid(itime) + initialTime - - m_scaledVars[times](itime); + for (int itime = 0; itime < m_numGridPoints - 1; ++itime) { + m_constraints.initial_time(Slice(), itime) = + m_scaledVars[initial_time](itime + 1) - + m_scaledVars[initial_time](itime); + m_constraints.final_time(Slice(), itime) = + m_scaledVars[final_time](itime + 1) - + m_scaledVars[final_time](itime); } // parameters @@ -603,12 +622,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { MXVector costOut; info.endpoint_function->call( - {m_unscaledVars[times](0), + {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), - m_unscaledVars[times](-1), + m_unscaledVars[final_time](0), m_unscaledVars[states](Slice(), -1), m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), @@ -677,12 +696,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { MXVector endpointOut; info.endpoint_function->call( - {m_unscaledVars[times](0), + {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), - m_unscaledVars[times](-1), + m_unscaledVars[final_time](0), m_unscaledVars[states](Slice(), -1), m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), @@ -704,7 +723,8 @@ Solution Transcription::solve(const Iterate& guessOrig) { // Resample the guess. // ------------------- - const auto guessTimes = createTimes(guessOrig.variables.at(times)); + const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), + guessOrig.variables.at(final_time)); auto guess = guessOrig.resample(guessTimes); // Adjust guesses for the slack variables to ensure they are the correct @@ -789,7 +809,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { if (m_solver.getOptimSolver() == "fatrop") { options["structure_detection"] = "auto"; std::vector equality; - for (int i = 0; i < 7*m_numMeshIntervals - 1; ++i) { + for (int i = 0; i < 9*m_numMeshIntervals; ++i) { equality.push_back(true); } options["equality"] = equality; @@ -947,13 +967,13 @@ void Transcription::printConstraintValues(const Iterate& it, const auto& vars = it.variables; const auto& lower = m_lowerBounds; const auto& upper = m_upperBounds; - print_bounds("State bounds", it.state_names, vars.at(times), + print_bounds("State bounds", it.state_names, it.times, vars.at(states), lower.at(states), upper.at(states)); - print_bounds("Control bounds", it.control_names, vars.at(times), + print_bounds("Control bounds", it.control_names, it.times, vars.at(controls), lower.at(controls), upper.at(controls)); - print_bounds("Multiplier bounds", it.multiplier_names, vars.at(times), + print_bounds("Multiplier bounds", it.multiplier_names, it.times, vars.at(multipliers), lower.at(multipliers), upper.at(multipliers)); - print_bounds("Derivative bounds", it.derivative_names, vars.at(times), + print_bounds("Derivative bounds", it.derivative_names, it.times, vars.at(derivatives), lower.at(derivatives), upper.at(derivatives)); // Need to update times for the slacks: // print_bounds("Slack bounds", it.slack_names, it.times, vars.at(slacks), @@ -1040,16 +1060,16 @@ void Transcription::printConstraintValues(const Iterate& it, } }; casadi::DM timeValues(2, 1); - timeValues(0) = vars.at(times)(0); - timeValues(1) = vars.at(times)(-1); + timeValues(0) = vars.at(initial_time)(0); + timeValues(1) = vars.at(final_time)(0); casadi::DM timeLower(2, 1); - timeLower(0) = lower.at(times)(0); - timeLower(1) = lower.at(times)(-1); + timeLower(0) = lower.at(initial_time)(0); + timeLower(1) = lower.at(final_time)(0); casadi::DM timeUpper(2, 1); - timeUpper(0) = upper.at(times)(0); - timeUpper(1) = upper.at(times)(-1); + timeUpper(0) = upper.at(initial_time)(0); + timeUpper(1) = upper.at(final_time)(0); printParameterBounds( "Time bounds", time_names, timeValues, timeLower, timeUpper); @@ -1089,7 +1109,7 @@ void Transcription::printConstraintValues(const Iterate& it, int argmax; double max = calcL1Norm(row, argmax); const double L1 = max; - const double time_of_max = vars.at(times)(argmax).scalar(); + const double time_of_max = it.times(argmax).scalar(); ss << std::setw(maxNameLength) << it.state_names[istate] << spacer << std::setprecision(2) << std::scientific << std::setw(9) << L2 @@ -1119,7 +1139,7 @@ void Transcription::printConstraintValues(const Iterate& it, int argmax; double max = calcL1Norm(row, argmax); const double L1 = max; - const double time_of_max = vars.at(times)(argmax).scalar(); + const double time_of_max = it.times(argmax).scalar(); std::string label = kinconNames.at(ikc); ss << std::setfill('0') << std::setw(2) << ikc << ":" @@ -1140,7 +1160,7 @@ void Transcription::printConstraintValues(const Iterate& it, for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { ss << std::setfill('0') << std::setw(3) << imesh << " "; ss.fill(' '); - ss << std::setw(9) << vars.at(times)(imesh).scalar() << " "; + ss << std::setw(9) << it.times(imesh).scalar() << " "; for (int ikc = 0; ikc < (int)kinconNames.size(); ++ikc) { const auto& value = constraints.kinematic(ikc, imesh).scalar(); ss << std::setprecision(2) << std::scientific @@ -1180,7 +1200,7 @@ void Transcription::printConstraintValues(const Iterate& it, int argmax; double max = calcL1Norm(row, argmax); const double L1 = max; - const double time_of_max = vars.at(times)(argmax).scalar(); + const double time_of_max = it.times(argmax).scalar(); std::string label = fmt::format("{}_{}", pc.name, ieq); ss << std::setfill('0') << std::setw(2) << ipc << ":" @@ -1202,7 +1222,7 @@ void Transcription::printConstraintValues(const Iterate& it, for (int ipcp = 0; ipcp < m_numPathConstraintPoints; ++ipcp) { ss << std::setfill('0') << std::setw(3) << ipcp << " "; ss.fill(' '); - ss << std::setw(9) << vars.at(times)(ipcp).scalar() << " "; + ss << std::setw(9) << it.times(ipcp).scalar() << " "; for (int ipc = 0; ipc < (int)pathconNames.size(); ++ipc) { const auto& value = constraints.path[ipc](ipcp).scalar(); ss << std::setprecision(2) << std::scientific @@ -1265,8 +1285,8 @@ Iterate Transcription::createInitialGuessFromBounds() const { setToMidpoint(kv.second, m_lowerBounds.at(kv.first), m_upperBounds.at(kv.first)); } - casGuess.variables[times] = createTimes( - casGuess.variables[times]); + casGuess.times = createTimes( + casGuess.variables[initial_time], casGuess.variables[final_time]); return casGuess; } @@ -1293,8 +1313,8 @@ Iterate Transcription::createRandomIterateWithinBounds( setRandom(kv.second, m_lowerBounds.at(kv.first), m_upperBounds.at(kv.first)); } - casIterate.variables[times] = createTimes( - casIterate.variables[times]); + casIterate.times = createTimes(casIterate.variables[initial_time], + casIterate.variables[final_time]); return casIterate; } @@ -1308,7 +1328,7 @@ casadi::MXVector Transcription::evalOnTrajectory( // Assemble input. // Add 1 for time input and 1 for parameters input. MXVector mxIn(inputs.size() + 2); - mxIn[0] = m_unscaledVars.at(times)(timeIndices); + mxIn[0] = m_times(timeIndices); for (int i = 0; i < (int)inputs.size(); ++i) { if (inputs[i] == multibody_states) { const auto NQ = m_problem.getNumCoordinates(); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index caae97d260..6c414d841d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -38,8 +38,8 @@ class Transcription { Iterate createRandomIterateWithinBounds( const SimTK::Random* = nullptr) const; template - T createTimes(const T& times) const { - return (times(-1) - times(0)) * m_grid + times(0); + T createTimes(const T& initial_time, const T& final_time) const { + return (final_time(0) - initial_time(0)) * m_grid + initial_time(0); } casadi::DM createQuadratureCoefficients() const { return createQuadratureCoefficientsImpl(); @@ -124,7 +124,8 @@ class Transcription { template struct Constraints { - T times; + T initial_time; + T final_time; T parameters; T defects; T multibody_residuals; @@ -151,11 +152,12 @@ class Transcription { int m_numPointsPerMeshInterval = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; - int m_numTimeConstraints = -1; + int m_numParameterConstraints = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; casadi::DM m_grid; casadi::DM m_pointsForInterpControls; + casadi::MX m_times; casadi::MX m_duration; // std::vector m_controlPoints; @@ -195,8 +197,8 @@ class Transcription { virtual casadi::DM createMeshIndicesImpl() const = 0; /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. - virtual void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const = 0; + virtual void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + casadi::MX& defects) const = 0; virtual void calcInterpolatingControlsImpl(const casadi::MX& /*controls*/, casadi::MX& /*interpControls*/) const { OPENSIM_THROW_IF(m_pointsForInterpControls.numel(), OpenSim::Exception, @@ -213,8 +215,8 @@ class Transcription { void transcribe(); void setObjectiveAndEndpointConstraints(); void calcDefects() { - calcDefectsImpl(m_unscaledVars.at(times), m_unscaledVars.at(states), - m_xdot, m_constraints.defects); + calcDefectsImpl(m_unscaledVars.at(states), m_xdot, + m_constraints.defects); } void calcInterpolatingControls() { calcInterpolatingControlsImpl( @@ -229,7 +231,8 @@ class Transcription { template static std::vector getSortedVarKeys(const Variables& vars) { std::vector keys; - keys.push_back(times); + keys.push_back(initial_time); + keys.push_back(final_time); keys.push_back(parameters); keys.push_back(states); keys.push_back(controls); @@ -255,7 +258,10 @@ class Transcription { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(times)[igrid + i]); + stdvec.push_back(vars.at(initial_time)[igrid + i]); + } + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(final_time)[igrid + i]); } for (int i = 0; i < N; ++i) { stdvec.push_back(vars.at(states)[igrid + i]); @@ -264,7 +270,8 @@ class Transcription { stdvec.push_back(vars.at(controls)[igrid + i]); } } - stdvec.push_back(vars.at(times)[m_numGridPoints - 1]); + stdvec.push_back(vars.at(initial_time)[m_numGridPoints - 1]); + stdvec.push_back(vars.at(final_time)[m_numGridPoints - 1]); stdvec.push_back(vars.at(states)[m_numGridPoints - 1]); stdvec.push_back(vars.at(controls)[m_numGridPoints - 1]); @@ -277,7 +284,10 @@ class Transcription { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(times)(casadi::Slice(), igrid + i)); + stdvec.push_back(vars.at(initial_time)(casadi::Slice(), igrid + i)); + } + for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(final_time)(casadi::Slice(), igrid + i)); } for (int i = 0; i < N; ++i) { stdvec.push_back(vars.at(states)(casadi::Slice(), igrid + i)); @@ -286,7 +296,8 @@ class Transcription { stdvec.push_back(vars.at(controls)(casadi::Slice(), igrid + i)); } } - stdvec.push_back(vars.at(times)(casadi::Slice(), m_numGridPoints - 1)); + stdvec.push_back(vars.at(initial_time)(casadi::Slice(), m_numGridPoints - 1)); + stdvec.push_back(vars.at(final_time)(casadi::Slice(), m_numGridPoints - 1)); stdvec.push_back(vars.at(states)(casadi::Slice(), m_numGridPoints - 1)); stdvec.push_back(vars.at(controls)(casadi::Slice(), m_numGridPoints - 1)); @@ -422,16 +433,18 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int icon = 0; - int itime = 0; + int itime_i = 0; + int itime_f = 0; int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; // Time constraints. for (int i = 0; i < N; ++i) { - if (igrid + i != 0 && igrid + i != m_numGridPoints - 1) { - copyColumn(constraints.times, itime++); - } + copyColumn(constraints.initial_time, itime_i++); + } + for (int i = 0; i < N; ++i) { + copyColumn(constraints.final_time, itime_f++); } // Parameter constraints. @@ -502,6 +515,9 @@ class Transcription { return T(casadi::Sparsity::dense(numRows, numColumns)); }; Constraints out; + out.initial_time = init(1, m_numParameterConstraints); + out.final_time = init(1, m_numParameterConstraints); + out.parameters = init(m_numParameterConstraints, m_numMeshPoints - 1); out.defects = init(m_numDefectsPerMeshInterval, m_numMeshPoints - 1); out.multibody_residuals = init(m_numMultibodyResiduals, m_numGridPoints); @@ -539,16 +555,18 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int icon = 0; - int itime = 0; + int itime_i = 0; + int itime_f = 0; int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; // Time constraints. for (int i = 0; i < N; ++i) { - if (igrid + i != 0 && igrid + i != m_numGridPoints - 1) { - copyColumn(out.times, itime++); - } + copyColumn(out.initial_time, itime_i++); + } + for (int i = 0; i < N; ++i) { + copyColumn(out.final_time, itime_f++); } // Parameter constraints. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index 2655ec8d42..6e6ba550fa 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -40,14 +40,14 @@ DM Trapezoidal::createMeshIndicesImpl() const { return DM::ones(1, m_numGridPoints); } -void Trapezoidal::calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, +void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, casadi::MX& defects) const { // We have arranged the code this way so that all constraints at a given // mesh point are grouped together (organizing the sparsity of the Jacobian // this way might have benefits for sparse linear algebra). for (int itime = 0; itime < m_numMeshIntervals; ++itime) { - const auto h = times(itime + 1) - times(itime); + const auto h = m_times(itime + 1) - m_times(itime); const auto x_i = x(Slice(), itime); const auto x_ip1 = x(Slice(), itime + 1); const auto xdot_i = xdot(Slice(), itime); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index d47bbbe6d4..443e947312 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -48,7 +48,7 @@ class Trapezoidal : public Transcription { casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& times, const casadi::MX& x, + void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, casadi::MX& defects) const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index a8bb5abf80..ae27a8d287 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -73,7 +73,22 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, CasOC::Iterate casIt; CasOC::VariablesDM& casVars = casIt.variables; using CasOC::Var; - casVars[Var::times] = convertToCasADiDMTranspose(mocoIt.getTime()); + + const auto& initialTime = mocoIt.getInitialTime(); + SimTK::Matrix initialTimeTrajectory(mocoIt.getNumTimes(), 1); + for (int i = 0; i < mocoIt.getNumTimes(); ++i) { + initialTimeTrajectory.updRow(i) = initialTime; + } + casVars[Var::initial_time] = + convertToCasADiDMTranspose(initialTimeTrajectory); + + const auto& finalTime = mocoIt.getFinalTime(); + SimTK::Matrix finalTimeTrajectory(mocoIt.getNumTimes(), 1); + for (int i = 0; i < mocoIt.getNumTimes(); ++i) { + finalTimeTrajectory.updRow(i) = finalTime; + } + casVars[Var::final_time] = convertToCasADiDMTranspose(finalTimeTrajectory); + casVars[Var::states] = convertToCasADiDMTranspose(mocoIt.getStatesTrajectory()); @@ -221,7 +236,7 @@ TOut convertToMocoTrajectory(const CasOC::Iterate& casIt, casVars.at(Var::parameters)(casadi::Slice(), 0); simtkParameters = convertToSimTKVector(paramsValue); } - SimTK::Vector simtkTimes = convertToSimTKVector(casVars.at(Var::times)); + SimTK::Vector simtkTimes = convertToSimTKVector(casIt.times); TOut mocoTraj(simtkTimes, casIt.state_names, controlNames, inputControlNames, casIt.multiplier_names, derivativeNames, From 9a5a29dec970739634a6f019805485fe6ff736e7 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 7 Aug 2024 16:16:56 -0700 Subject: [PATCH 09/38] First basic problem working (!) --- .../exampleSlidingMass/exampleSlidingMass.cpp | 23 +-- .../Moco/MocoCasADiSolver/CasOCFunction.cpp | 2 +- OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h | 2 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 1 + .../MocoCasADiSolver/CasOCHermiteSimpson.h | 1 + .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 43 +++-- .../MocoCasADiSolver/CasOCLegendreGauss.h | 1 + .../CasOCLegendreGaussRadau.cpp | 1 + .../CasOCLegendreGaussRadau.h | 1 + .../MocoCasADiSolver/CasOCTranscription.cpp | 165 +++++++++++------- .../MocoCasADiSolver/CasOCTranscription.h | 93 ++++------ .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 5 +- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 5 +- 13 files changed, 193 insertions(+), 150 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index ef9808f623..f1aa6a5bcb 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -53,11 +53,11 @@ std::unique_ptr createSlidingMassModel() { coord.setName("position"); model->addComponent(joint); - auto* actu = new CoordinateActuator(); - actu->setCoordinate(&coord); - actu->setName("actuator"); - actu->setOptimalForce(1); - model->addComponent(actu); + // auto* actu = new CoordinateActuator(); + // actu->setCoordinate(&coord); + // actu->setName("actuator"); + // actu->setOptimalForce(1); + // model->addComponent(actu); body->attachGeometry(new Sphere(0.05)); @@ -90,22 +90,23 @@ int main() { MocoInitialBounds(0), MocoFinalBounds(1)); // Speed must be within [-50, 50] throughout the motion. // Initial and final speed must be 0. Use compact syntax. - problem.setStateInfo("/slider/position/speed", {-50, 50}, 0, 0); + problem.setStateInfo("/slider/position/speed", {-50, 50}); // Applied force must be between -50 and 50. - problem.setControlInfo("/actuator", MocoBounds(-50, 50)); + // problem.setControlInfo("/actuator", MocoBounds(-50, 50)); // Cost. // ----- - problem.addGoal(); + // problem.addGoal(); // Configure the solver. // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); - solver.set_parallel(0); - solver.set_optim_solver("ipopt"); - // solver.set_transcription_scheme("legendre-gauss-1"); + // solver.set_parallel(0); + solver.set_optim_solver("fatrop"); + solver.set_optim_hessian_approximation("exact"); + solver.set_transcription_scheme("hermite-simpson"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index c75c03830e..0e3c794acc 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -317,7 +317,7 @@ casadi::DM VelocityCorrection::getSubsetPoint( const int NMBS = m_casProblem->getNumStates() - m_casProblem->getNumAuxiliaryStates(); if (i == 0) { - return fullPoint.at(initial_time)(0); + return fullPoint.at(initial_time)(itime); } else if (i == 1) { return fullPoint.at(states)(Slice(0, NMBS), itime); } else if (i == 2) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index 386d2301df..155b86788d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -88,7 +88,7 @@ class Function : public casadi::Callback { int itime = 0; using casadi::Slice; if (i == 0) { - return fullPoint.at(initial_time)(0); + return fullPoint.at(initial_time)(itime); } else if (i == 1) { return fullPoint.at(states)(Slice(), itime); } else if (i == 2) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 5d74f89de7..c43f6162dd 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -51,6 +51,7 @@ DM HermiteSimpson::createMeshIndicesImpl() const { } void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, const casadi::MX& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 567fc5db04..95a46bdb3b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -74,6 +74,7 @@ class HermiteSimpson : public Transcription { casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 3129c5935e..0993cce6fb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -54,28 +54,45 @@ DM LegendreGauss::createMeshIndicesImpl() const { return indices; } -void LegendreGauss::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, + casadi::MX& defects) const { // For more information, see doxygen documentation for the class. + const int NP = m_problem.getNumParameters(); const int NS = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - const int igrid = imesh * (m_degree + 1); - const auto h = m_times(igrid + m_degree + 1) - m_times(igrid); - const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); - const auto xdot_i = xdot(Slice(), - Slice(igrid + 1, igrid + m_degree + 1)); - const auto x_ip1 = x(Slice(), igrid + m_degree + 1); + const int i = imesh * (m_degree + 1); + const int ip1 = i + m_degree + 1; + const auto h = m_delta_t(imesh); + const auto x_i = x(Slice(), Slice(i, ip1)); + const auto xdot_i = xdot(Slice(), Slice(i + 1, ip1)); + const auto x_ip1 = x(Slice(), ip1); + + // End state interpolation. + defects(Slice(0, NS), imesh) = + x_ip1 - MX::mtimes(x_i, m_interpolationCoefficients); + + // Initial time. + defects(Slice(NS, NS + 1), imesh) = ti(imesh + 1) - ti(imesh); + + // Final time. + defects(Slice(NS + 1, NS + 2), imesh) = tf(imesh + 1) - tf(imesh); + + // Parameters. + if (NP) { + const auto p_i = p(Slice(), imesh); + const auto p_ip1 = p(Slice(), imesh + 1); + defects(Slice(NS + 2, NS + 2 + NP), imesh) = p_ip1 - p_i; + } // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); for (int d = 0; d < m_degree; ++d) { - defects(Slice(d * NS, (d + 1) * NS), imesh) = residual(Slice(), d); + const int istart = (d + 1) * NS + 2 + NP; + const int iend = (d + 2) * NS + 2 + NP; + defects(Slice(istart, iend), imesh) = residual(Slice(), d); } - - // End state interpolation. - defects(Slice(m_degree * NS, (m_degree + 1) * NS), imesh) = - x_ip1 - MX::mtimes(x_i, m_interpolationCoefficients); } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 0738f660b0..091f2fa547 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -116,6 +116,7 @@ class LegendreGauss : public Transcription { casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 4713138e64..17ce0d5c3b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -53,6 +53,7 @@ DM LegendreGaussRadau::createMeshIndicesImpl() const { } void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, const casadi::MX& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 4413adce6b..446ba97219 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -122,6 +122,7 @@ class LegendreGaussRadau : public Transcription { casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index e10034ad99..119b4a9911 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -95,9 +95,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numMeshPoints = (int)m_solver.getMesh().size(); m_numGridPoints = (int)grid.numel(); m_numMeshIntervals = m_numMeshPoints - 1; - m_numParameterConstraints = m_numGridPoints - 1; + m_numParameterConstraints = m_numMeshIntervals; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; - m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; + m_numDefectsPerMeshInterval = numDefectsPerMeshInterval + + m_problem.getNumParameters() + 2; m_numPointsPerMeshInterval = numPointsPerMeshInterval; m_pointsForInterpControls = pointsForInterpControls; // m_controlPoints = controlPoints; @@ -107,7 +108,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); m_numConstraints = - (m_problem.getNumParameters() + 2) * m_numParameterConstraints + + // (m_problem.getNumParameters() + 2) * m_numMeshIntervals + m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + @@ -135,14 +136,18 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // OpenSim::Exception, // "The number of control points must match the number of grid " // "points."); - for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { + + for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { m_scaledVectorVars[initial_time].push_back( - MX::sym("initial_time_" + std::to_string(igrid), 1, 1)); + MX::sym("initial_time_" + std::to_string(imesh), 1, 1)); m_scaledVectorVars[final_time].push_back( - MX::sym("final_time_" + std::to_string(igrid), 1, 1)); + MX::sym("final_time_" + std::to_string(imesh), 1, 1)); m_scaledVectorVars[parameters].push_back( - MX::sym("parameters_" + std::to_string(igrid), + MX::sym("parameters_" + std::to_string(imesh), m_problem.getNumParameters(), 1)); + } + + for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { m_scaledVectorVars[states].push_back( MX::sym("states_" + std::to_string(igrid), m_problem.getNumStates(), 1)); @@ -235,8 +240,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); // TODO use inf bounds - setVariableBounds(initial_time, 0, Slice(1, m_numGridPoints), {-100, 100}); - setVariableBounds(final_time, 0, Slice(1, m_numGridPoints), {-100, 100}); + setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), {-100, 100}); + setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), {-100, 100}); setVariableScaling(initial_time, 0, Slice(), m_problem.getTimeInitialBounds()); @@ -321,10 +326,37 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } m_unscaledVars = unscaleVariables(m_scaledVars); - m_times = createTimes( - m_unscaledVars[initial_time], m_unscaledVars[final_time]); - m_duration = m_unscaledVars[final_time](0) - - m_unscaledVars[initial_time](0); + // + // for (int itime = 0; itime < m_numGridPoints; ++itime) { + // m_times(itime) = (m_unscaledVars[final_time](itime) - + // m_unscaledVars[initial_time](itime)) * m_grid(itime) + + // m_unscaledVars[initial_time](itime); + // } + + m_times = MX(casadi::Sparsity::dense(1, m_numGridPoints)); + m_parameters = MX(casadi::Sparsity::dense( + m_problem.getNumParameters(), m_numGridPoints)); + int N = m_numPointsPerMeshInterval - 1; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + for (int i = 0; i < N; ++i) { + m_parameters(Slice(), igrid + i) = + m_unscaledVars[parameters](Slice(), imesh); + m_times(igrid + i) = (m_unscaledVars[final_time](imesh) - + m_unscaledVars[initial_time](imesh)) * + m_grid(igrid + i) + + m_unscaledVars[initial_time](imesh); + } + } + m_parameters(Slice(), -1) = m_unscaledVars[parameters](Slice(), -1); + m_times(-1) = (m_unscaledVars[final_time](-1) - + m_unscaledVars[initial_time](-1)) * + m_grid(-1) + + m_unscaledVars[initial_time](-1); + + // m_times = createTimes(m_unscaledVars[initial_time], m_unscaledVars[final_time]); + m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; + m_delta_t = m_duration / m_numMeshIntervals; // calcExtrapolatedControls(); } @@ -346,30 +378,30 @@ void Transcription::transcribe() { // TODO: Does creating all this memory have efficiency implications // in CasADi? - // Initialize memory for time constraints. - // --------------------------------------- - m_constraints.initial_time = - MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); - m_constraintsLowerBounds.initial_time = - DM::zeros(1, m_numParameterConstraints); - m_constraintsUpperBounds.initial_time = - DM::zeros(1, m_numParameterConstraints); - - m_constraints.final_time = - MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); - m_constraintsLowerBounds.final_time = - DM::zeros(1, m_numParameterConstraints); - m_constraintsUpperBounds.final_time = - DM::zeros(1, m_numParameterConstraints); - - // Initialize memory for parameter constraints. - // -------------------------------------------- - m_constraints.parameters = MX(casadi::Sparsity::dense( - m_problem.getNumParameters(), m_numGridPoints - 1)); - m_constraintsLowerBounds.parameters = - DM::zeros(m_problem.getNumParameters(), m_numGridPoints - 1); - m_constraintsUpperBounds.parameters = - DM::zeros(m_problem.getNumParameters(), m_numGridPoints - 1); + // // Initialize memory for time constraints. + // // --------------------------------------- + // m_constraints.initial_time = + // MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); + // m_constraintsLowerBounds.initial_time = + // DM::zeros(1, m_numParameterConstraints); + // m_constraintsUpperBounds.initial_time = + // DM::zeros(1, m_numParameterConstraints); + + // m_constraints.final_time = + // MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); + // m_constraintsLowerBounds.final_time = + // DM::zeros(1, m_numParameterConstraints); + // m_constraintsUpperBounds.final_time = + // DM::zeros(1, m_numParameterConstraints); + + // // Initialize memory for parameter constraints. + // // -------------------------------------------- + // m_constraints.parameters = MX(casadi::Sparsity::dense( + // m_problem.getNumParameters(), m_numParameterConstraints)); + // m_constraintsLowerBounds.parameters = + // DM::zeros(m_problem.getNumParameters(), m_numParameterConstraints); + // m_constraintsUpperBounds.parameters = + // DM::zeros(m_problem.getNumParameters(), m_numParameterConstraints); // Initialize memory for state derivatives and defects. // ---------------------------------------------------- @@ -412,24 +444,24 @@ void Transcription::transcribe() { m_constraintsUpperBounds.kinematic = casadi::DM::repmat( kcBounds.upper, numKinematicConstraints, m_numMeshPoints); - // time - // ---- - for (int itime = 0; itime < m_numGridPoints - 1; ++itime) { - m_constraints.initial_time(Slice(), itime) = - m_scaledVars[initial_time](itime + 1) - - m_scaledVars[initial_time](itime); - m_constraints.final_time(Slice(), itime) = - m_scaledVars[final_time](itime + 1) - - m_scaledVars[final_time](itime); - } - - // parameters - // ---------- - for (int itime = 0; itime < m_numGridPoints - 1; ++itime) { - m_constraints.parameters(Slice(), itime) = - m_scaledVars[parameters](Slice(), itime + 1) - - m_scaledVars[parameters](Slice(), itime); - } + // // time + // // ---- + // for (int itime = 0; itime < m_numParameterConstraints; ++itime) { + // m_constraints.initial_time(Slice(), itime) = + // m_scaledVars[initial_time](itime + 1) - + // m_scaledVars[initial_time](itime); + // m_constraints.final_time(Slice(), itime) = + // m_scaledVars[final_time](itime + 1) - + // m_scaledVars[final_time](itime); + // } + + // // parameters + // // ---------- + // for (int itime = 0; itime < m_numParameterConstraints; ++itime) { + // m_constraints.parameters(Slice(), itime) = + // m_scaledVars[parameters](Slice(), itime + 1) - + // m_scaledVars[parameters](Slice(), itime); + // } // qdot // ---- @@ -615,7 +647,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { {states, controls, multipliers, derivatives}, m_gridIndices) .at(0); - integral = m_duration * dot(quadCoeffs.T(), integrandTraj); + integral = m_duration(0) * dot(quadCoeffs.T(), integrandTraj); } else { integral = MX::nan(1, 1); } @@ -644,7 +676,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { const double multiplierWeight = m_solver.getLagrangeMultiplierWeight(); // Sum across constraints of each multiplier element squared. MX integrandTraj = MX::sum1(MX::sq(mults)); - m_objectiveTerms(iterm++) = multiplierWeight * m_duration * + m_objectiveTerms(iterm++) = multiplierWeight * m_duration(0) * dot(quadCoeffs.T(), integrandTraj); } @@ -655,7 +687,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { const double accelWeight = m_solver.getImplicitMultibodyAccelerationsWeight(); MX integrandTraj = MX::sum1(MX::sq(accels)); - m_objectiveTerms(iterm++) = accelWeight * m_duration * + m_objectiveTerms(iterm++) = accelWeight * m_duration(0) * dot(quadCoeffs.T(), integrandTraj); } @@ -668,7 +700,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { const double auxDerivWeight = m_solver.getImplicitAuxiliaryDerivativesWeight(); MX integrandTraj = MX::sum1(MX::sq(auxDerivs)); - m_objectiveTerms(iterm++) = auxDerivWeight * m_duration * + m_objectiveTerms(iterm++) = auxDerivWeight * m_duration(0) * dot(quadCoeffs.T(), integrandTraj); } @@ -689,7 +721,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { {states, controls, multipliers, derivatives}, m_gridIndices) .at(0); - integral = m_duration * dot(quadCoeffs.T(), integrandTraj); + integral = m_duration(-1) * dot(quadCoeffs.T(), integrandTraj); } else { integral = MX::nan(1, 1); } @@ -806,11 +838,14 @@ Solution Transcription::solve(const Iterate& guessOrig) { jacobian.sparsity().to_file( prefix + "constraint_Jacobian_sparsity.mtx"); } + + const auto lbg = flattenConstraints(m_constraintsLowerBounds); + const auto ubg = flattenConstraints(m_constraintsUpperBounds); if (m_solver.getOptimSolver() == "fatrop") { options["structure_detection"] = "auto"; std::vector equality; - for (int i = 0; i < 9*m_numMeshIntervals; ++i) { - equality.push_back(true); + for (int i = 0; i < m_numConstraints; ++i) { + equality.push_back(lbg(i).scalar() == ubg(i).scalar()); } options["equality"] = equality; options["debug"] = true; @@ -825,8 +860,8 @@ Solution Transcription::solve(const Iterate& guessOrig) { {"x0", flattenVariablesDM(scaleVariables(guess.variables))}, {"lbx", flattenVariablesDM(scaleVariables(m_lowerBounds))}, {"ubx", flattenVariablesDM(scaleVariables(m_upperBounds))}, - {"lbg", flattenConstraints(m_constraintsLowerBounds)}, - {"ubg", flattenConstraints(m_constraintsUpperBounds)}}); + {"lbg", lbg}, + {"ubg", ubg}}); // Create a CasOC::Solution. // ------------------------- @@ -1341,7 +1376,7 @@ casadi::MXVector Transcription::evalOnTrajectory( mxIn[i + 1] = m_unscaledVars.at(inputs[i])(Slice(), timeIndices); } } - mxIn[mxIn.size() - 1] = m_unscaledVars.at(parameters)(Slice(), timeIndices); + mxIn[mxIn.size() - 1] = m_parameters(Slice(), timeIndices); MXVector mxOut; trajFunc.call(mxIn, mxOut); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 6c414d841d..365555134f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -124,9 +124,9 @@ class Transcription { template struct Constraints { - T initial_time; - T final_time; - T parameters; + // T initial_time; + // T final_time; + // T parameters; T defects; T multibody_residuals; T auxiliary_residuals; @@ -158,6 +158,8 @@ class Transcription { casadi::DM m_grid; casadi::DM m_pointsForInterpControls; casadi::MX m_times; + casadi::MX m_parameters; + casadi::MX m_delta_t; casadi::MX m_duration; // std::vector m_controlPoints; @@ -198,6 +200,7 @@ class Transcription { /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. virtual void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const = 0; virtual void calcInterpolatingControlsImpl(const casadi::MX& /*controls*/, casadi::MX& /*interpControls*/) const { @@ -216,7 +219,8 @@ class Transcription { void setObjectiveAndEndpointConstraints(); void calcDefects() { calcDefectsImpl(m_unscaledVars.at(states), m_xdot, - m_constraints.defects); + m_unscaledVars.at(initial_time), m_unscaledVars.at(final_time), + m_unscaledVars.at(parameters), m_constraints.defects); } void calcInterpolatingControls() { calcInterpolatingControlsImpl( @@ -257,22 +261,19 @@ class Transcription { int N = m_numPointsPerMeshInterval - 1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(initial_time)[igrid + i]); - } - for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(final_time)[igrid + i]); - } - for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(states)[igrid]); + stdvec.push_back(vars.at(initial_time)[imesh]); + stdvec.push_back(vars.at(final_time)[imesh]); + for (int i = 1; i < N; ++i) { stdvec.push_back(vars.at(states)[igrid + i]); } for (int i = 0; i < N; ++i) { stdvec.push_back(vars.at(controls)[igrid + i]); } } - stdvec.push_back(vars.at(initial_time)[m_numGridPoints - 1]); - stdvec.push_back(vars.at(final_time)[m_numGridPoints - 1]); stdvec.push_back(vars.at(states)[m_numGridPoints - 1]); + stdvec.push_back(vars.at(initial_time)[m_numMeshIntervals]); + stdvec.push_back(vars.at(final_time)[m_numMeshIntervals]); stdvec.push_back(vars.at(controls)[m_numGridPoints - 1]); return casadi::MX::veccat(stdvec); @@ -283,22 +284,19 @@ class Transcription { int N = m_numPointsPerMeshInterval - 1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(initial_time)(casadi::Slice(), igrid + i)); - } - for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(final_time)(casadi::Slice(), igrid + i)); - } - for (int i = 0; i < N; ++i) { + stdvec.push_back(vars.at(states)(casadi::Slice(), igrid)); + stdvec.push_back(vars.at(initial_time)(casadi::Slice(), imesh)); + stdvec.push_back(vars.at(final_time)(casadi::Slice(), imesh)); + for (int i = 1; i < N; ++i) { stdvec.push_back(vars.at(states)(casadi::Slice(), igrid + i)); } for (int i = 0; i < N; ++i) { stdvec.push_back(vars.at(controls)(casadi::Slice(), igrid + i)); } } - stdvec.push_back(vars.at(initial_time)(casadi::Slice(), m_numGridPoints - 1)); - stdvec.push_back(vars.at(final_time)(casadi::Slice(), m_numGridPoints - 1)); stdvec.push_back(vars.at(states)(casadi::Slice(), m_numGridPoints - 1)); + stdvec.push_back(vars.at(initial_time)(casadi::Slice(), m_numMeshIntervals)); + stdvec.push_back(vars.at(final_time)(casadi::Slice(), m_numMeshIntervals)); stdvec.push_back(vars.at(controls)(casadi::Slice(), m_numGridPoints - 1)); return casadi::DM::veccat(stdvec); @@ -433,28 +431,20 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int icon = 0; - int itime_i = 0; - int itime_f = 0; int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - // Time constraints. - for (int i = 0; i < N; ++i) { - copyColumn(constraints.initial_time, itime_i++); - } - for (int i = 0; i < N; ++i) { - copyColumn(constraints.final_time, itime_f++); - } - - // Parameter constraints. - for (int i = 0; i < N; ++i) { - copyColumn(constraints.parameters, iparam++); - } - // Defect constraints. copyColumn(constraints.defects, imesh); + // // Time constraints. + // copyColumn(constraints.initial_time, imesh); + // copyColumn(constraints.final_time, imesh); + + // // Parameter constraints. + // copyColumn(constraints.parameters, imesh); + // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { copyColumn(constraints.multibody_residuals, igrid + i); @@ -515,9 +505,10 @@ class Transcription { return T(casadi::Sparsity::dense(numRows, numColumns)); }; Constraints out; - out.initial_time = init(1, m_numParameterConstraints); - out.final_time = init(1, m_numParameterConstraints); - out.parameters = init(m_numParameterConstraints, m_numMeshPoints - 1); + // out.initial_time = init(1, m_numParameterConstraints); + // out.final_time = init(1, m_numParameterConstraints); + // out.parameters = init(m_problem.getNumParameters(), + // m_numParameterConstraints); out.defects = init(m_numDefectsPerMeshInterval, m_numMeshPoints - 1); out.multibody_residuals = init(m_numMultibodyResiduals, m_numGridPoints); @@ -555,28 +546,20 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int icon = 0; - int itime_i = 0; - int itime_f = 0; int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - // Time constraints. - for (int i = 0; i < N; ++i) { - copyColumn(out.initial_time, itime_i++); - } - for (int i = 0; i < N; ++i) { - copyColumn(out.final_time, itime_f++); - } - - // Parameter constraints. - for (int i = 0; i < N; ++i) { - copyColumn(out.parameters, iparam++); - } - // Defect constraints. copyColumn(out.defects, imesh); + // // Time constraints. + // copyColumn(out.initial_time, imesh); + // copyColumn(out.final_time, imesh); + + // // Parameter constraints. + // copyColumn(out.parameters, imesh); + // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { copyColumn(out.multibody_residuals, igrid + i); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index 6e6ba550fa..31cc3089e9 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -40,8 +40,9 @@ DM Trapezoidal::createMeshIndicesImpl() const { return DM::ones(1, m_numGridPoints); } -void Trapezoidal::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const { +void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, + casadi::MX& defects) const { // We have arranged the code this way so that all constraints at a given // mesh point are grouped together (organizing the sparsity of the Jacobian diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index 443e947312..e6548a1bc4 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -48,8 +48,9 @@ class Trapezoidal : public Transcription { casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, - const casadi::MX& xdot, casadi::MX& defects) const override; + void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, + const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, + casadi::MX& defects) const override; }; } // namespace CasOC From f47d6bf7f10c8c88e01fa364bb4a5244f6a6be82 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 7 Aug 2024 16:59:37 -0700 Subject: [PATCH 10/38] Remove unneeded code for time and parameter constraints --- .../exampleSlidingMass/exampleSlidingMass.cpp | 14 +++--- .../MocoCasADiSolver/CasOCTranscription.cpp | 44 ------------------- .../MocoCasADiSolver/CasOCTranscription.h | 26 +---------- 3 files changed, 9 insertions(+), 75 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index f1aa6a5bcb..350489ffe2 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -53,11 +53,11 @@ std::unique_ptr createSlidingMassModel() { coord.setName("position"); model->addComponent(joint); - // auto* actu = new CoordinateActuator(); - // actu->setCoordinate(&coord); - // actu->setName("actuator"); - // actu->setOptimalForce(1); - // model->addComponent(actu); + auto* actu = new CoordinateActuator(); + actu->setCoordinate(&coord); + actu->setName("actuator"); + actu->setOptimalForce(1); + model->addComponent(actu); body->attachGeometry(new Sphere(0.05)); @@ -93,7 +93,7 @@ int main() { problem.setStateInfo("/slider/position/speed", {-50, 50}); // Applied force must be between -50 and 50. - // problem.setControlInfo("/actuator", MocoBounds(-50, 50)); + problem.setControlInfo("/actuator", MocoBounds(-50, 50)); // Cost. // ----- @@ -106,7 +106,7 @@ int main() { // solver.set_parallel(0); solver.set_optim_solver("fatrop"); solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("hermite-simpson"); + solver.set_transcription_scheme("legendre-gauss-2"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 119b4a9911..bce1d5a82a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -378,31 +378,6 @@ void Transcription::transcribe() { // TODO: Does creating all this memory have efficiency implications // in CasADi? - // // Initialize memory for time constraints. - // // --------------------------------------- - // m_constraints.initial_time = - // MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); - // m_constraintsLowerBounds.initial_time = - // DM::zeros(1, m_numParameterConstraints); - // m_constraintsUpperBounds.initial_time = - // DM::zeros(1, m_numParameterConstraints); - - // m_constraints.final_time = - // MX(casadi::Sparsity::dense(1, m_numParameterConstraints)); - // m_constraintsLowerBounds.final_time = - // DM::zeros(1, m_numParameterConstraints); - // m_constraintsUpperBounds.final_time = - // DM::zeros(1, m_numParameterConstraints); - - // // Initialize memory for parameter constraints. - // // -------------------------------------------- - // m_constraints.parameters = MX(casadi::Sparsity::dense( - // m_problem.getNumParameters(), m_numParameterConstraints)); - // m_constraintsLowerBounds.parameters = - // DM::zeros(m_problem.getNumParameters(), m_numParameterConstraints); - // m_constraintsUpperBounds.parameters = - // DM::zeros(m_problem.getNumParameters(), m_numParameterConstraints); - // Initialize memory for state derivatives and defects. // ---------------------------------------------------- m_xdot = MX(NS, m_numGridPoints); @@ -444,25 +419,6 @@ void Transcription::transcribe() { m_constraintsUpperBounds.kinematic = casadi::DM::repmat( kcBounds.upper, numKinematicConstraints, m_numMeshPoints); - // // time - // // ---- - // for (int itime = 0; itime < m_numParameterConstraints; ++itime) { - // m_constraints.initial_time(Slice(), itime) = - // m_scaledVars[initial_time](itime + 1) - - // m_scaledVars[initial_time](itime); - // m_constraints.final_time(Slice(), itime) = - // m_scaledVars[final_time](itime + 1) - - // m_scaledVars[final_time](itime); - // } - - // // parameters - // // ---------- - // for (int itime = 0; itime < m_numParameterConstraints; ++itime) { - // m_constraints.parameters(Slice(), itime) = - // m_scaledVars[parameters](Slice(), itime + 1) - - // m_scaledVars[parameters](Slice(), itime); - // } - // qdot // ---- const MX u = m_unscaledVars[states](Slice(NQ, NQ + NU), Slice()); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 365555134f..51d4c4affc 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -124,9 +124,6 @@ class Transcription { template struct Constraints { - // T initial_time; - // T final_time; - // T parameters; T defects; T multibody_residuals; T auxiliary_residuals; @@ -161,7 +158,8 @@ class Transcription { casadi::MX m_parameters; casadi::MX m_delta_t; casadi::MX m_duration; - // std::vector m_controlPoints; + std::vector m_controlPoints; + casadi::MX m_controls; private: VariablesMXVector m_scaledVectorVars; @@ -431,20 +429,12 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int icon = 0; - int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; // Defect constraints. copyColumn(constraints.defects, imesh); - // // Time constraints. - // copyColumn(constraints.initial_time, imesh); - // copyColumn(constraints.final_time, imesh); - - // // Parameter constraints. - // copyColumn(constraints.parameters, imesh); - // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { copyColumn(constraints.multibody_residuals, igrid + i); @@ -505,10 +495,6 @@ class Transcription { return T(casadi::Sparsity::dense(numRows, numColumns)); }; Constraints out; - // out.initial_time = init(1, m_numParameterConstraints); - // out.final_time = init(1, m_numParameterConstraints); - // out.parameters = init(m_problem.getNumParameters(), - // m_numParameterConstraints); out.defects = init(m_numDefectsPerMeshInterval, m_numMeshPoints - 1); out.multibody_residuals = init(m_numMultibodyResiduals, m_numGridPoints); @@ -546,20 +532,12 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int icon = 0; - int iparam = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; // Defect constraints. copyColumn(out.defects, imesh); - // // Time constraints. - // copyColumn(out.initial_time, imesh); - // copyColumn(out.final_time, imesh); - - // // Parameter constraints. - // copyColumn(out.parameters, imesh); - // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { copyColumn(out.multibody_residuals, igrid + i); From c5041ab82e3eab31b36767adb90bdf444258b757 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 8 Aug 2024 17:14:02 -0700 Subject: [PATCH 11/38] Provide interface for variable ordering in transcription schemes --- .../exampleSlidingMass/exampleSlidingMass.cpp | 18 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 19 +- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 7 +- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 49 ++++- .../MocoCasADiSolver/CasOCLegendreGauss.h | 1 + .../CasOCLegendreGaussRadau.cpp | 4 +- .../Moco/MocoCasADiSolver/CasOCProblem.cpp | 31 +++- OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp | 1 + .../MocoCasADiSolver/CasOCTranscription.cpp | 21 +-- .../MocoCasADiSolver/CasOCTranscription.h | 172 +++++++++--------- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 5 +- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 17 +- 12 files changed, 206 insertions(+), 139 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 350489ffe2..014804b056 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -53,11 +53,11 @@ std::unique_ptr createSlidingMassModel() { coord.setName("position"); model->addComponent(joint); - auto* actu = new CoordinateActuator(); - actu->setCoordinate(&coord); - actu->setName("actuator"); - actu->setOptimalForce(1); - model->addComponent(actu); + // auto* actu = new CoordinateActuator(); + // actu->setCoordinate(&coord); + // actu->setName("actuator"); + // actu->setOptimalForce(1); + // model->addComponent(actu); body->attachGeometry(new Sphere(0.05)); @@ -93,7 +93,7 @@ int main() { problem.setStateInfo("/slider/position/speed", {-50, 50}); // Applied force must be between -50 and 50. - problem.setControlInfo("/actuator", MocoBounds(-50, 50)); + // problem.setControlInfo("/actuator", MocoBounds(-50, 50)); // Cost. // ----- @@ -103,10 +103,10 @@ int main() { // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); - // solver.set_parallel(0); + solver.set_parallel(0); solver.set_optim_solver("fatrop"); - solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("legendre-gauss-2"); + // solver.set_optim_hessian_approximation("exact"); + solver.set_transcription_scheme("legendre-gauss-3"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index c43f6162dd..fae6116bc7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -51,11 +51,12 @@ DM HermiteSimpson::createMeshIndicesImpl() const { } void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, - const casadi::MX& xdot, casadi::MX& defects) const { + const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, + const casadi::MX& p, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); + const int NP = m_problem.getNumParameters(); int time_i; int time_mid; @@ -68,7 +69,7 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, time_mid = 2 * imesh + 1; time_ip1 = 2 * imesh + 2; - const auto h = m_times(time_ip1) - m_times(time_i); + const auto h = m_intervals(imesh); const auto x_i = x(Slice(), time_i); const auto x_mid = x(Slice(), time_mid); const auto x_ip1 = x(Slice(), time_ip1); @@ -76,12 +77,20 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, const auto xdot_mid = xdot(Slice(), time_mid); const auto xdot_ip1 = xdot(Slice(), time_ip1); + // Time variables. + defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh); + defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh); + + // Parameters. + defects(Slice(2, 2 + NP), imesh) = + p(Slice(), imesh + 1) - p(Slice(), imesh); + // Hermite interpolant defects. - defects(Slice(0, NS), imesh) = + defects(Slice(2 + NP, 2 + NP + NS), imesh) = x_mid - 0.5 * (x_ip1 + x_i) - (h / 8.0) * (xdot_i - xdot_ip1); // Simpson integration defects. - defects(Slice(NS, 2 * NS), imesh) = + defects(Slice(2 + NP + NS, 2 + NP + 2*NS), imesh) = x_ip1 - x_i - (h / 6.0) * (xdot_ip1 + 4.0 * xdot_mid + xdot_i); } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 0997108b0e..d8e4b69c19 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -46,11 +46,13 @@ enum Var { template using Variables = std::unordered_map>; +template +using VectorVariables = std::unordered_map, std::hash>; /// Numeric variables for initial guesses and solutions. using VariablesDM = Variables; /// Symbolic variables, used to define the problem. -using VariablesMXVector = Variables; +using VectorVariablesMX = VectorVariables; using VariablesMX = Variables; /// This struct is used to obtain initial guesses. @@ -67,6 +69,9 @@ struct Iterate { /// Return a new iterate in which the data is resampled at the times in /// newTimes. Iterate resample(const casadi::DM& newTimes) const; + // Populate the iterate with values at each mesh point for all parameters + // (including initial and final times). + void populateParameters(int numMeshPoints); }; /// This struct is used to return a solution to a problem. Use `stats` diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 0993cce6fb..f93be588d8 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -59,12 +59,12 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. - const int NP = m_problem.getNumParameters(); const int NS = m_problem.getNumStates(); + const int NP = m_problem.getNumParameters(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int i = imesh * (m_degree + 1); const int ip1 = i + m_degree + 1; - const auto h = m_delta_t(imesh); + const auto h = m_intervals(imesh); const auto x_i = x(Slice(), Slice(i, ip1)); const auto xdot_i = xdot(Slice(), Slice(i + 1, ip1)); const auto x_ip1 = x(Slice(), ip1); @@ -73,18 +73,13 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, defects(Slice(0, NS), imesh) = x_ip1 - MX::mtimes(x_i, m_interpolationCoefficients); - // Initial time. + // Time variables. defects(Slice(NS, NS + 1), imesh) = ti(imesh + 1) - ti(imesh); - - // Final time. defects(Slice(NS + 1, NS + 2), imesh) = tf(imesh + 1) - tf(imesh); // Parameters. - if (NP) { - const auto p_i = p(Slice(), imesh); - const auto p_ip1 = p(Slice(), imesh + 1); - defects(Slice(NS + 2, NS + 2 + NP), imesh) = p_ip1 - p_i; - } + defects(Slice(NS + 2, NS + 2 + NP), imesh) = + p(Slice(), imesh + 1) - p(Slice(), imesh); // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); @@ -113,6 +108,40 @@ void LegendreGauss::calcInterpolatingControlsImpl( } } +std::vector> LegendreGauss::getVariableOrder() const { + std::vector> order; + int N = m_numPointsPerMeshInterval - 1; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + order.push_back({states, igrid}); + order.push_back({initial_time, imesh}); + order.push_back({final_time, imesh}); + order.push_back({parameters, imesh}); + for (int i = 1; i < N; ++i) { + order.push_back({states, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({controls, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({multipliers, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({derivatives, igrid + i}); + } + order.push_back({slacks, imesh}); + } + order.push_back({states, m_numGridPoints - 1}); + order.push_back({initial_time, m_numMeshIntervals}); + order.push_back({final_time, m_numMeshIntervals}); + order.push_back({parameters, m_numMeshIntervals}); + order.push_back({controls, m_numGridPoints - 1}); + order.push_back({multipliers, m_numGridPoints - 1}); + order.push_back({derivatives, m_numGridPoints - 1}); + + return order; +} + // void LegendreGauss::calcExtrapolatedControlsImpl(casadi::MX& controls) const { // if (m_problem.getNumControls()) { // // TODO diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 091f2fa547..eee5cb8c88 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -120,6 +120,7 @@ class LegendreGauss : public Transcription { casadi::MX& defects) const override; void calcInterpolatingControlsImpl(const casadi::MX& controls, casadi::MX& interpControls) const override; + std::vector> getVariableOrder() const override; // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; int m_degree; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 17ce0d5c3b..8ddd5ea5c8 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -53,8 +53,8 @@ DM LegendreGaussRadau::createMeshIndicesImpl() const { } void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, - const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, - const casadi::MX& xdot, casadi::MX& defects) const { + const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, + const casadi::MX& p, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp index 740cf59782..2fb599e458 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp @@ -32,8 +32,8 @@ using OpenSim::Exception; namespace CasOC { Iterate Iterate::resample(const casadi::DM& newTimes) const { - // Since we are converting to a MocoTrajectory and immediately converting - // back to a CasOC::Iterate after resampling, we do not need to provide + // Since we are converting to a MocoTrajectory and immediately converting + // back to a CasOC::Iterate after resampling, we do not need to provide // Input control indexes, even if they are present in the MocoProblem. auto mocoTraj = OpenSim::convertToMocoTrajectory(*this); auto simtkNewTimes = OpenSim::convertToSimTKVector(newTimes); @@ -41,6 +41,33 @@ Iterate Iterate::resample(const casadi::DM& newTimes) const { return OpenSim::convertToCasOCIterate(mocoTraj); } +void Iterate::populateParameters(int numMeshPoints) { + if (variables.at(Var::initial_time).size2() != numMeshPoints) { + const auto& initial_time = variables.at(Var::initial_time)(0); + variables[Var::initial_time] = casadi::DM::zeros(1, numMeshPoints); + variables[Var::initial_time](0, casadi::Slice()) = initial_time; + } + + if (variables.at(Var::final_time).size2() != numMeshPoints) { + const auto& final_time = variables.at(Var::final_time)(0); + variables[Var::final_time] = casadi::DM::zeros(1, numMeshPoints); + variables[Var::final_time](0, casadi::Slice()) = final_time; + } + + int numParameters = variables.at(Var::parameters).size1(); + if (numParameters) { + if (variables.at(Var::parameters).size2() != numMeshPoints) { + const auto& parameters = + variables.at(Var::parameters)(casadi::Slice(), 0); + variables[Var::parameters] = + casadi::DM::zeros(numParameters, numMeshPoints); + for (int i = 0; i < numMeshPoints; ++i) { + variables[Var::parameters](casadi::Slice(), i) = parameters; + } + } + } +} + std::vector Problem::createKinematicConstraintEquationNamesImpl() const { std::vector names(getNumKinematicConstraintEquations()); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp index 1054bc854e..aecfb61d06 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp @@ -126,6 +126,7 @@ Solution Solver::solve(const Iterate& guess) const { guessCopy.variables.at(initial_time), guessCopy.variables.at(final_time)); guessCopy = guessCopy.resample(guessTimes); + guessCopy.populateParameters(static_cast(m_mesh.size())); pointsForSparsityDetection->push_back(guessCopy.variables); } else if (m_sparsity_detection == "random") { // Make sure the exact same sparsity pattern is used every time. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index bce1d5a82a..e4a531581a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -326,12 +326,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } m_unscaledVars = unscaleVariables(m_scaledVars); - // - // for (int itime = 0; itime < m_numGridPoints; ++itime) { - // m_times(itime) = (m_unscaledVars[final_time](itime) - - // m_unscaledVars[initial_time](itime)) * m_grid(itime) + - // m_unscaledVars[initial_time](itime); - // } m_times = MX(casadi::Sparsity::dense(1, m_numGridPoints)); m_parameters = MX(casadi::Sparsity::dense( @@ -354,9 +348,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_grid(-1) + m_unscaledVars[initial_time](-1); - // m_times = createTimes(m_unscaledVars[initial_time], m_unscaledVars[final_time]); m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; - m_delta_t = m_duration / m_numMeshIntervals; + m_intervals = m_duration / m_numMeshIntervals; // calcExtrapolatedControls(); } @@ -714,6 +707,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), guessOrig.variables.at(final_time)); auto guess = guessOrig.resample(guessTimes); + guess.populateParameters(m_numMeshPoints); // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). @@ -754,7 +748,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { options[m_solver.getOptimSolver()] = m_solver.getSolverOptions(); } - auto x = flattenVariablesMX(m_scaledVectorVars); + auto x = flattenVariables(m_scaledVectorVars); casadi_int numVariables = x.numel(); // The m_constraints symbolic vector holds all of the expressions for @@ -813,9 +807,9 @@ Solution Transcription::solve(const Iterate& guessOrig) { // -------------------------------------------------------- // The inputs and outputs of nlpFunc are numeric (casadi::DM). const casadi::DMDict nlpResult = nlpFunc(casadi::DMDict{ - {"x0", flattenVariablesDM(scaleVariables(guess.variables))}, - {"lbx", flattenVariablesDM(scaleVariables(m_lowerBounds))}, - {"ubx", flattenVariablesDM(scaleVariables(m_upperBounds))}, + {"x0", flattenVariables(scaleVariables(guess.variables))}, + {"lbx", flattenVariables(scaleVariables(m_lowerBounds))}, + {"ubx", flattenVariables(scaleVariables(m_upperBounds))}, {"lbg", lbg}, {"ubg", ubg}}); @@ -831,6 +825,9 @@ Solution Transcription::solve(const Iterate& guessOrig) { casadi::DMVector objectiveOut; objectiveFunc.call(finalVarsDMV, objectiveOut); solution.objective_breakdown = expandObjectiveTerms(objectiveOut[0]); + + solution.times = createTimes( + solution.variables[initial_time], solution.variables[final_time]); solution.stats = nlpFunc.stats(); // Print breakdown of objective. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 51d4c4affc..c997c7bb9b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -156,13 +156,13 @@ class Transcription { casadi::DM m_pointsForInterpControls; casadi::MX m_times; casadi::MX m_parameters; - casadi::MX m_delta_t; + casadi::MX m_intervals; casadi::MX m_duration; std::vector m_controlPoints; casadi::MX m_controls; private: - VariablesMXVector m_scaledVectorVars; + VectorVariablesMX m_scaledVectorVars; VariablesMX m_scaledVars; VariablesMX m_unscaledVars; VariablesDM m_lowerBounds; @@ -213,108 +213,116 @@ class Transcription { // "Must provide scheme for extrapolated controls.") // } - void transcribe(); - void setObjectiveAndEndpointConstraints(); - void calcDefects() { - calcDefectsImpl(m_unscaledVars.at(states), m_xdot, - m_unscaledVars.at(initial_time), m_unscaledVars.at(final_time), - m_unscaledVars.at(parameters), m_constraints.defects); - } - void calcInterpolatingControls() { - calcInterpolatingControlsImpl( - m_unscaledVars.at(controls), m_constraints.interp_controls); - } - // void calcExtrapolatedControls() { - // calcExtrapolatedControlsImpl(m_scaledVars.at(controls)); - // } - /// Use this function to ensure you iterate through variables in the same - /// order. - template - static std::vector getSortedVarKeys(const Variables& vars) { - std::vector keys; - keys.push_back(initial_time); - keys.push_back(final_time); - keys.push_back(parameters); - keys.push_back(states); - keys.push_back(controls); - keys.push_back(multipliers); - keys.push_back(derivatives); - // for (const auto& kv : vars) { keys.push_back(kv.first); } - // std::sort(keys.begin(), keys.end()); - return keys; - } - /// Convert the map of variables into a column vector, for passing onto - /// nlpsol(), etc. - // template - // static T flattenVariables(const CasOC::Variables& vars) { - // std::vector stdvec; - // for (const auto& key : getSortedVarKeys(vars)) { - // stdvec.push_back(vars.at(key)); - // } - // return T::veccat(stdvec); - // } - casadi::MX flattenVariablesMX(const CasOC::VariablesMXVector& vars) const { - std::vector stdvec; + /// order. This may be overridden in derived classes to provide + /// scheme-specific sparsity patterns. + virtual std::vector> getVariableOrder() const { + std::vector> order; int N = m_numPointsPerMeshInterval - 1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - stdvec.push_back(vars.at(states)[igrid]); - stdvec.push_back(vars.at(initial_time)[imesh]); - stdvec.push_back(vars.at(final_time)[imesh]); - for (int i = 1; i < N; ++i) { - stdvec.push_back(vars.at(states)[igrid + i]); + order.push_back({initial_time, imesh}); + order.push_back({final_time, imesh}); + order.push_back({parameters, imesh}); + for (int i = 0; i < N; ++i) { + order.push_back({states, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({controls, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({multipliers, igrid + i}); } for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(controls)[igrid + i]); + order.push_back({derivatives, igrid + i}); } + order.push_back({slacks, imesh}); } - stdvec.push_back(vars.at(states)[m_numGridPoints - 1]); - stdvec.push_back(vars.at(initial_time)[m_numMeshIntervals]); - stdvec.push_back(vars.at(final_time)[m_numMeshIntervals]); - stdvec.push_back(vars.at(controls)[m_numGridPoints - 1]); - - return casadi::MX::veccat(stdvec); + order.push_back({initial_time, m_numMeshIntervals}); + order.push_back({final_time, m_numMeshIntervals}); + order.push_back({parameters, m_numMeshIntervals}); + order.push_back({states, m_numGridPoints - 1}); + order.push_back({controls, m_numGridPoints - 1}); + order.push_back({multipliers, m_numGridPoints - 1}); + order.push_back({derivatives, m_numGridPoints - 1}); + + return order; } + /// Convert the map of variables into a column vector, for passing onto + /// nlpsol(), etc. + casadi::MX flattenVariables(const VectorVariablesMX& vars) const { + std::vector stdvec; + auto flatten = [&stdvec, vars](Var var, int index) { + stdvec.push_back(vars.at(var)[index]); + }; + + auto varOrder = getVariableOrder(); + for (const auto& kv : varOrder) { + flatten(kv.first, kv.second); + } - casadi::DM flattenVariablesDM(const CasOC::VariablesDM& vars) const { + return casadi::MX::vertcat(stdvec); + } + /// Convert the map of variables into a column vector, for passing onto + /// nlpsol(), etc. + casadi::DM flattenVariables(const VariablesDM& vars) const { std::vector stdvec; - int N = m_numPointsPerMeshInterval - 1; - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - int igrid = imesh * N; - stdvec.push_back(vars.at(states)(casadi::Slice(), igrid)); - stdvec.push_back(vars.at(initial_time)(casadi::Slice(), imesh)); - stdvec.push_back(vars.at(final_time)(casadi::Slice(), imesh)); - for (int i = 1; i < N; ++i) { - stdvec.push_back(vars.at(states)(casadi::Slice(), igrid + i)); - } - for (int i = 0; i < N; ++i) { - stdvec.push_back(vars.at(controls)(casadi::Slice(), igrid + i)); + auto flatten = [this, &stdvec, vars](Var var, int index) { + if (m_scaledVars.at(var).rows()) { + stdvec.push_back(vars.at(var)(casadi::Slice(), index)); } + }; + + auto varOrder = getVariableOrder(); + for (const auto& kv : varOrder) { + flatten(kv.first, kv.second); } - stdvec.push_back(vars.at(states)(casadi::Slice(), m_numGridPoints - 1)); - stdvec.push_back(vars.at(initial_time)(casadi::Slice(), m_numMeshIntervals)); - stdvec.push_back(vars.at(final_time)(casadi::Slice(), m_numMeshIntervals)); - stdvec.push_back(vars.at(controls)(casadi::Slice(), m_numGridPoints - 1)); - return casadi::DM::veccat(stdvec); + return casadi::DM::vertcat(stdvec); } /// Convert the 'x' column vector into separate variables. - CasOC::VariablesDM expandVariables(const casadi::DM& x) const { - CasOC::VariablesDM out; + VariablesDM expandVariables(const casadi::DM& x) const { + VariablesDM out; using casadi::Slice; casadi_int offset = 0; - for (const auto& key : getSortedVarKeys(m_scaledVars)) { - const auto& value = m_scaledVars.at(key); - // Convert a portion of the column vector into a matrix. - out[key] = casadi::DM::reshape( - x(Slice(offset, offset + value.numel())), value.rows(), - value.columns()); - offset += value.numel(); + for (const auto& kv : m_scaledVars) { + out[kv.first] = casadi::DM::zeros(kv.second.rows(), + kv.second.columns()); + } + + auto expand = [this, &out, &offset, x](Var var, int index) { + casadi_int size = m_scaledVars.at(var).rows(); + if (size) { + out[var](Slice(), index) = casadi::DM::reshape( + x(Slice(offset, offset + size)), size, 1); + offset += size; + } + }; + + auto varOrder = getVariableOrder(); + for (const auto& kv : varOrder) { + expand(kv.first, kv.second); } + return out; } + void transcribe(); + void setObjectiveAndEndpointConstraints(); + void calcDefects() { + calcDefectsImpl(m_unscaledVars.at(states), m_xdot, + m_unscaledVars.at(initial_time), m_unscaledVars.at(final_time), + m_unscaledVars.at(parameters), m_constraints.defects); + } + void calcInterpolatingControls() { + calcInterpolatingControlsImpl( + m_unscaledVars.at(controls), m_constraints.interp_controls); + } + // void calcExtrapolatedControls() { + // calcExtrapolatedControlsImpl(m_scaledVars.at(controls)); + // } + + /// unscaled = (upper - lower) * scaled - 0.5 * (upper + lower); template Variables unscaleVariables(const Variables& scaledVars) { diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 87c914e91f..69217c3dd6 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -411,9 +411,12 @@ MocoSolution MocoCasADiSolver::solveImpl() const { checkConstraintJacobianRank(mocoSolution); } + std::string return_status = (get_optim_solver() == "ipopt") ? + "return_status" : "unified_return_status"; + const long long elapsed = stopwatch.getElapsedTimeInNs(); setSolutionStats(mocoSolution, casSolution.stats.at("success"), - casSolution.objective, casSolution.stats.at("return_status"), + casSolution.objective, casSolution.stats.at(return_status), casSolution.stats.at("iter_count"), SimTK::nsToSec(elapsed), casSolution.objective_breakdown); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index ae27a8d287..02d7eeef44 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -74,21 +74,8 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoIt, CasOC::VariablesDM& casVars = casIt.variables; using CasOC::Var; - const auto& initialTime = mocoIt.getInitialTime(); - SimTK::Matrix initialTimeTrajectory(mocoIt.getNumTimes(), 1); - for (int i = 0; i < mocoIt.getNumTimes(); ++i) { - initialTimeTrajectory.updRow(i) = initialTime; - } - casVars[Var::initial_time] = - convertToCasADiDMTranspose(initialTimeTrajectory); - - const auto& finalTime = mocoIt.getFinalTime(); - SimTK::Matrix finalTimeTrajectory(mocoIt.getNumTimes(), 1); - for (int i = 0; i < mocoIt.getNumTimes(); ++i) { - finalTimeTrajectory.updRow(i) = finalTime; - } - casVars[Var::final_time] = convertToCasADiDMTranspose(finalTimeTrajectory); - + casVars[Var::initial_time] = mocoIt.getInitialTime(); + casVars[Var::final_time] = mocoIt.getFinalTime(); casVars[Var::states] = convertToCasADiDMTranspose(mocoIt.getStatesTrajectory()); From e46812649ef9192dec6f5add86685d1fbf5f2409 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 9 Aug 2024 11:57:38 -0700 Subject: [PATCH 12/38] Add time and parameter defects to all transcription schemes --- .../exampleSlidingMass/exampleSlidingMass.cpp | 4 +-- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 5 ++- .../CasOCLegendreGaussRadau.cpp | 13 +++++++- .../MocoCasADiSolver/CasOCTranscription.h | 32 +++++++------------ .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 25 +++++++++++---- 5 files changed, 46 insertions(+), 33 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 014804b056..bd0f348311 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -104,9 +104,9 @@ int main() { MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); solver.set_parallel(0); - solver.set_optim_solver("fatrop"); + solver.set_optim_solver("ipopt"); // solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("legendre-gauss-3"); + solver.set_transcription_scheme("legendre-gauss-radau-3"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index fae6116bc7..3f370411b1 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -55,12 +55,11 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, const casadi::MX& p, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. - const int NS = m_problem.getNumStates(); - const int NP = m_problem.getNumParameters(); - int time_i; int time_mid; int time_ip1; + const int NS = m_problem.getNumStates(); + const int NP = m_problem.getNumParameters(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { time_i = 2 * imesh; // Needed for defects and path constraints. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 8ddd5ea5c8..7bca13634f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -58,6 +58,7 @@ void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); + const int NP = m_problem.getNumParameters(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; const auto h = m_times(igrid + m_degree) - m_times(igrid); @@ -65,10 +66,20 @@ void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, const auto xdot_i = xdot(Slice(), Slice(igrid + 1, igrid + m_degree + 1)); + // Time variables. + defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh); + defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh); + + // Parameters. + defects(Slice(2, 2 + NP), imesh) = + p(Slice(), imesh + 1) - p(Slice(), imesh); + // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); for (int d = 0; d < m_degree; ++d) { - defects(Slice(d * NS, (d + 1) * NS), imesh) = residual(Slice(), d); + const int istart = d * NS + 2 + NP; + const int iend = (d + 1) * NS + 2 + NP; + defects(Slice(istart, iend), imesh) = residual(Slice(), d); } } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index c997c7bb9b..17eb063414 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -214,8 +214,10 @@ class Transcription { // } /// Use this function to ensure you iterate through variables in the same - /// order. This may be overridden in derived classes to provide - /// scheme-specific sparsity patterns. + /// order. Returns a vector whose elements are pairs of variable keys and + /// trajectory indexes. The vector should have length equal to the length of + /// the flattened variable vector passed to nlpsol(). This may be overridden + /// in derived classes to provide scheme-specific sparsity patterns. virtual std::vector> getVariableOrder() const { std::vector> order; int N = m_numPointsPerMeshInterval - 1; @@ -252,13 +254,9 @@ class Transcription { /// nlpsol(), etc. casadi::MX flattenVariables(const VectorVariablesMX& vars) const { std::vector stdvec; - auto flatten = [&stdvec, vars](Var var, int index) { - stdvec.push_back(vars.at(var)[index]); - }; - auto varOrder = getVariableOrder(); for (const auto& kv : varOrder) { - flatten(kv.first, kv.second); + stdvec.push_back(vars.at(kv.first)[kv.second]); } return casadi::MX::vertcat(stdvec); @@ -267,15 +265,11 @@ class Transcription { /// nlpsol(), etc. casadi::DM flattenVariables(const VariablesDM& vars) const { std::vector stdvec; - auto flatten = [this, &stdvec, vars](Var var, int index) { - if (m_scaledVars.at(var).rows()) { - stdvec.push_back(vars.at(var)(casadi::Slice(), index)); - } - }; - auto varOrder = getVariableOrder(); for (const auto& kv : varOrder) { - flatten(kv.first, kv.second); + if (m_scaledVars.at(kv.first).rows()) { + stdvec.push_back(vars.at(kv.first)(casadi::Slice(), kv.second)); + } } return casadi::DM::vertcat(stdvec); @@ -290,18 +284,16 @@ class Transcription { kv.second.columns()); } - auto expand = [this, &out, &offset, x](Var var, int index) { + auto varOrder = getVariableOrder(); + for (const auto& kv : varOrder) { + const auto& var = kv.first; + const auto& index = kv.second; casadi_int size = m_scaledVars.at(var).rows(); if (size) { out[var](Slice(), index) = casadi::DM::reshape( x(Slice(offset, offset + size)), size, 1); offset += size; } - }; - - auto varOrder = getVariableOrder(); - for (const auto& kv : varOrder) { - expand(kv.first, kv.second); } return out; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index 31cc3089e9..36a7e3f76c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -47,15 +47,26 @@ void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, // We have arranged the code this way so that all constraints at a given // mesh point are grouped together (organizing the sparsity of the Jacobian // this way might have benefits for sparse linear algebra). - for (int itime = 0; itime < m_numMeshIntervals; ++itime) { - const auto h = m_times(itime + 1) - m_times(itime); - const auto x_i = x(Slice(), itime); - const auto x_ip1 = x(Slice(), itime + 1); - const auto xdot_i = xdot(Slice(), itime); - const auto xdot_ip1 = xdot(Slice(), itime + 1); + const int NS = m_problem.getNumStates(); + const int NP = m_problem.getNumParameters(); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + const auto h = m_intervals(imesh); + const auto x_i = x(Slice(), imesh); + const auto x_ip1 = x(Slice(), imesh + 1); + const auto xdot_i = xdot(Slice(), imesh); + const auto xdot_ip1 = xdot(Slice(), imesh + 1); + + // Time variables. + defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh); + defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh); + + // Parameters. + defects(Slice(2, 2 + NP), imesh) = + p(Slice(), imesh + 1) - p(Slice(), imesh); // Trapezoidal defects. - defects(Slice(), itime) = x_ip1 - (x_i + 0.5 * h * (xdot_ip1 + xdot_i)); + defects(Slice(2 + NP, 2 + NP + NS), imesh) = + x_ip1 - (x_i + 0.5 * h * (xdot_ip1 + xdot_i)); } } From af3dd3597ce3ddfe566a95e13ef82b91191b8929 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 9 Aug 2024 12:57:24 -0700 Subject: [PATCH 13/38] Fix variable ordering for trapezoidal rule --- .../exampleSlidingMass/exampleSlidingMass.cpp | 6 ++-- .../MocoCasADiSolver/CasOCTranscription.h | 32 +++++++++---------- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 15 +++++++++ .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 1 + dependencies/CMakeLists.txt | 4 +-- 5 files changed, 37 insertions(+), 21 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index bd0f348311..77f837e908 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -103,10 +103,10 @@ int main() { // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); - solver.set_parallel(0); + // solver.set_parallel(0); solver.set_optim_solver("ipopt"); - // solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("legendre-gauss-radau-3"); + solver.set_optim_hessian_approximation("exact"); + solver.set_transcription_scheme("legendre-gauss-3"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 17eb063414..e78faf5011 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -250,6 +250,22 @@ class Transcription { return order; } + + void transcribe(); + void setObjectiveAndEndpointConstraints(); + void calcDefects() { + calcDefectsImpl(m_unscaledVars.at(states), m_xdot, + m_unscaledVars.at(initial_time), m_unscaledVars.at(final_time), + m_unscaledVars.at(parameters), m_constraints.defects); + } + void calcInterpolatingControls() { + calcInterpolatingControlsImpl( + m_unscaledVars.at(controls), m_constraints.interp_controls); + } + // void calcExtrapolatedControls() { + // calcExtrapolatedControlsImpl(m_scaledVars.at(controls)); + // } + /// Convert the map of variables into a column vector, for passing onto /// nlpsol(), etc. casadi::MX flattenVariables(const VectorVariablesMX& vars) const { @@ -299,22 +315,6 @@ class Transcription { return out; } - void transcribe(); - void setObjectiveAndEndpointConstraints(); - void calcDefects() { - calcDefectsImpl(m_unscaledVars.at(states), m_xdot, - m_unscaledVars.at(initial_time), m_unscaledVars.at(final_time), - m_unscaledVars.at(parameters), m_constraints.defects); - } - void calcInterpolatingControls() { - calcInterpolatingControlsImpl( - m_unscaledVars.at(controls), m_constraints.interp_controls); - } - // void calcExtrapolatedControls() { - // calcExtrapolatedControlsImpl(m_scaledVars.at(controls)); - // } - - /// unscaled = (upper - lower) * scaled - 0.5 * (upper + lower); template Variables unscaleVariables(const Variables& scaledVars) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index 36a7e3f76c..fc9954a516 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -70,4 +70,19 @@ void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, } } +std::vector> Trapezoidal::getVariableOrder() const { + std::vector> order; + for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { + order.push_back({initial_time, imesh}); + order.push_back({final_time, imesh}); + order.push_back({parameters, imesh}); + order.push_back({states, imesh}); + order.push_back({controls, imesh}); + order.push_back({multipliers, imesh}); + order.push_back({derivatives, imesh}); + } + + return order; +} + } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index e6548a1bc4..512fde4aff 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -51,6 +51,7 @@ class Trapezoidal : public Transcription { void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; + std::vector> getVariableOrder() const override; }; } // namespace CasOC diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index db3a322c8f..e9b5357662 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -358,7 +358,7 @@ if(SUPERBUILD_casadi) DEFAULT ON DEPENDS ipopt GIT_URL https://github.com/casadi/casadi.git - GIT_TAG 974f0d9d955b1a0b61d892a6b4c107c116aa8600 + GIT_TAG 3.6.6 CMAKE_ARGS -DWITH_IPOPT:BOOL=ON -DIPOPT_LIBRARIES:FILEPATH=ipopt.dll.lib -DIPOPT_INCLUDE_DIRS:PATH=${CMAKE_INSTALL_PREFIX}/ipopt/include/coin-or @@ -378,7 +378,7 @@ if(SUPERBUILD_casadi) DEFAULT ON DEPENDS ipopt GIT_URL https://github.com/casadi/casadi.git - GIT_TAG 974f0d9d955b1a0b61d892a6b4c107c116aa8600 + GIT_TAG 3.6.6 CMAKE_ARGS -DWITH_IPOPT:BOOL=ON -DWITH_THREAD:BOOL=ON -DWITH_BUILD_MUMPS:BOOL=OFF From f47938f375420044ade35083e614ae6f2bd0742e Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 9 Aug 2024 13:39:17 -0700 Subject: [PATCH 14/38] Clean up parameter repmatting --- .../Moco/MocoCasADiSolver/CasOCFunction.cpp | 2 -- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 6 ++-- .../Moco/MocoCasADiSolver/CasOCProblem.cpp | 34 ++++++------------- OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp | 4 ++- .../MocoCasADiSolver/CasOCTranscription.cpp | 10 +++--- .../MocoCasADiSolver/CasOCTranscription.h | 4 +++ 6 files changed, 26 insertions(+), 34 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index 0e3c794acc..cc663688f8 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -82,7 +82,6 @@ casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, // Split input into separate DMs. std::vector in(this->n_in()); { - int offset = 0; for (int iin = 0; iin < this->n_in(); ++iin) { OPENSIM_THROW_IF(this->size2_in(iin) != 1, OpenSim::Exception, "Internal error."); @@ -92,7 +91,6 @@ casadi::Sparsity Function::get_jac_sparsity(casadi_int oind, casadi_int iind, } else { in[iin] = casadi::DM::zeros(size, 1); } - offset += size; } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index d8e4b69c19..4663bdb6cf 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -69,9 +69,9 @@ struct Iterate { /// Return a new iterate in which the data is resampled at the times in /// newTimes. Iterate resample(const casadi::DM& newTimes) const; - // Populate the iterate with values at each mesh point for all parameters - // (including initial and final times). - void populateParameters(int numMeshPoints); + /// Make repeat copies of parameter variables (including initial and final + /// time) using the same value for each time point. + Iterate repmatParameters(int numPoints); }; /// This struct is used to return a solution to a problem. Use `stats` diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp index 2fb599e458..a6455aa1de 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp @@ -41,31 +41,17 @@ Iterate Iterate::resample(const casadi::DM& newTimes) const { return OpenSim::convertToCasOCIterate(mocoTraj); } -void Iterate::populateParameters(int numMeshPoints) { - if (variables.at(Var::initial_time).size2() != numMeshPoints) { - const auto& initial_time = variables.at(Var::initial_time)(0); - variables[Var::initial_time] = casadi::DM::zeros(1, numMeshPoints); - variables[Var::initial_time](0, casadi::Slice()) = initial_time; - } - - if (variables.at(Var::final_time).size2() != numMeshPoints) { - const auto& final_time = variables.at(Var::final_time)(0); - variables[Var::final_time] = casadi::DM::zeros(1, numMeshPoints); - variables[Var::final_time](0, casadi::Slice()) = final_time; - } +Iterate Iterate::repmatParameters(int numPoints) { + Iterate it(*this); + it.variables.at(Var::initial_time) = casadi::DM::repmat( + it.variables.at(Var::initial_time)(0), 1, numPoints); + it.variables.at(Var::final_time) = casadi::DM::repmat( + it.variables.at(Var::final_time)(0), 1, numPoints); + it.variables.at(Var::parameters) = casadi::DM::repmat( + it.variables.at(Var::parameters)(casadi::Slice(), 0), + it.variables.at(Var::parameters).size1(), numPoints); - int numParameters = variables.at(Var::parameters).size1(); - if (numParameters) { - if (variables.at(Var::parameters).size2() != numMeshPoints) { - const auto& parameters = - variables.at(Var::parameters)(casadi::Slice(), 0); - variables[Var::parameters] = - casadi::DM::zeros(numParameters, numMeshPoints); - for (int i = 0; i < numMeshPoints; ++i) { - variables[Var::parameters](casadi::Slice(), i) = parameters; - } - } - } + return it; } std::vector diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp index aecfb61d06..c09057d30e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.cpp @@ -21,6 +21,7 @@ #include "CasOCHermiteSimpson.h" #include "CasOCLegendreGauss.h" #include "CasOCLegendreGaussRadau.h" +#include "OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h" #include @@ -119,6 +120,7 @@ Solution Solver::solve(const Iterate& guess) const { auto transcription = createTranscription(); auto pointsForSparsityDetection = std::make_shared>(); + std::cout << "m_sparsity_detection: " << m_sparsity_detection << std::endl; if (m_sparsity_detection == "initial-guess") { // Interpolate the guess. Iterate guessCopy(guess); @@ -126,7 +128,7 @@ Solution Solver::solve(const Iterate& guess) const { guessCopy.variables.at(initial_time), guessCopy.variables.at(final_time)); guessCopy = guessCopy.resample(guessTimes); - guessCopy.populateParameters(static_cast(m_mesh.size())); + guessCopy = guessCopy.repmatParameters(static_cast(m_mesh.size())); pointsForSparsityDetection->push_back(guessCopy.variables); } else if (m_sparsity_detection == "random") { // Make sure the exact same sparsity pattern is used every time. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index e4a531581a..7f2f392c37 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -16,6 +16,7 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ #include "CasOCTranscription.h" +#include using casadi::DM; using casadi::MX; @@ -239,9 +240,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); - // TODO use inf bounds - setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), {-100, 100}); - setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), {-100, 100}); + setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), + {-casadi::inf, casadi::inf}); + setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), + {-casadi::inf, casadi::inf}); setVariableScaling(initial_time, 0, Slice(), m_problem.getTimeInitialBounds()); @@ -707,7 +709,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { const auto guessTimes = createTimes(guessOrig.variables.at(initial_time), guessOrig.variables.at(final_time)); auto guess = guessOrig.resample(guessTimes); - guess.populateParameters(m_numMeshPoints); + guess = guess.repmatParameters(m_numMeshPoints); // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index e78faf5011..56f5e155ee 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -19,6 +19,7 @@ * -------------------------------------------------------------------------- */ #include "CasOCSolver.h" +#include namespace CasOC { @@ -61,6 +62,9 @@ class Transcription { return meshIndices; } + casadi::DM repeatCopyParameter(const casadi::DM ¶meter) const { + return casadi::DM::repmat(parameter, 1, m_numMeshIntervals); + } Solution solve(const Iterate& guessOrig); From b84b58e429229c388d0f28839700b108aea89d7f Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 12 Aug 2024 07:27:34 -0700 Subject: [PATCH 15/38] Start implementing symbolic control interpolation --- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 75 +++++++++------ .../MocoCasADiSolver/CasOCHermiteSimpson.h | 23 ++--- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 73 ++++++++------ .../MocoCasADiSolver/CasOCLegendreGauss.h | 29 ++---- .../CasOCLegendreGaussRadau.cpp | 57 +++++++---- .../CasOCLegendreGaussRadau.h | 23 +++-- .../MocoCasADiSolver/CasOCTranscription.cpp | 72 ++++++-------- .../MocoCasADiSolver/CasOCTranscription.h | 96 ++++--------------- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 7 ++ .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 14 +-- 10 files changed, 216 insertions(+), 253 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 3f370411b1..2ecc3e6f49 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -95,38 +95,53 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, } void HermiteSimpson::calcInterpolatingControlsImpl( - const casadi::MX& controls, casadi::MX& interpControls) const { - if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { - int time_i; - int time_mid; - int time_ip1; - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - time_i = 2 * imesh; - time_mid = 2 * imesh + 1; - time_ip1 = 2 * imesh + 2; - const auto c_i = controls(Slice(), time_i); - const auto c_mid = controls(Slice(), time_mid); - const auto c_ip1 = controls(Slice(), time_ip1); - interpControls(Slice(), imesh) = c_mid - 0.5 * (c_ip1 + c_i); - } + const casadi::MX& controlVars, casadi::MX& controls) const { + + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + controls(Slice(), 2 * imesh) = controlVars(Slice(), imesh); + controls(Slice(), 2 * imesh + 1) = 0.5 * ( + controlVars(Slice(), imesh + 1) + controlVars(Slice(), imesh)); } + controls(Slice(), -1) = controlVars(Slice(), -1); } -// void HermiteSimpson::calcExtrapolatedControlsImpl(casadi::MX& controls) const { -// if (m_problem.getNumControls()) { -// int time_i; -// int time_mid; -// int time_ip1; -// for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { -// time_i = 2 * imesh; -// time_mid = 2 * imesh + 1; -// time_ip1 = 2 * imesh + 2; -// const auto c_i = controls(Slice(), time_i); -// const auto c_ip1 = controls(Slice(), time_ip1); -// controls(Slice(), time_mid) = 0.5 * (c_ip1 + c_i); -// } -// } -// } +std::vector> HermiteSimpson::getVariableOrder() const { + std::vector> order; + int N = m_numPointsPerMeshInterval - 1; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + order.push_back({initial_time, imesh}); + order.push_back({final_time, imesh}); + order.push_back({parameters, imesh}); + order.push_back({states, igrid}); + order.push_back({states, igrid + 1}); + if (m_solver.getInterpolateControlMidpoints()) { + order.push_back({controls, imesh}); + } else { + order.push_back({controls, igrid}); + order.push_back({controls, igrid + 1}); + } + + for (int i = 0; i < N; ++i) { + order.push_back({controls, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({multipliers, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({derivatives, igrid + i}); + } + order.push_back({slacks, imesh}); + } + order.push_back({initial_time, m_numMeshIntervals}); + order.push_back({final_time, m_numMeshIntervals}); + order.push_back({parameters, m_numMeshIntervals}); + order.push_back({states, m_numGridPoints - 1}); + order.push_back({controls, m_numControlPoints - 1}); + order.push_back({multipliers, m_numGridPoints - 1}); + order.push_back({derivatives, m_numGridPoints - 1}); + + return order; +} } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 95a46bdb3b..e697534ec0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -47,27 +47,18 @@ class HermiteSimpson : public Transcription { casadi::DM grid = casadi::DM::zeros(1, (2 * m_solver.getMesh().size()) - 1); const auto& mesh = m_solver.getMesh(); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); - casadi::DM pointsForInterpControls; - if (interpControls) { - pointsForInterpControls = - casadi::DM::zeros(1, m_solver.getMesh().size() - 1); - } - // std::vector controlPoints; + std::vector controlPoints; for (int i = 0; i < grid.numel(); ++i) { if (i % 2 == 0) { grid(i) = mesh[i / 2]; - // controlPoints.push_back(true); + controlPoints.push_back(true); } else { grid(i) = .5 * (mesh[i / 2] + mesh[i / 2 + 1]); - // controlPoints.push_back(false); - if (interpControls) { - pointsForInterpControls(i / 2) = grid(i); - } + controlPoints.push_back(false); } } createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), 3, - pointsForInterpControls); + controlPoints); } private: @@ -76,9 +67,9 @@ class HermiteSimpson : public Transcription { void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(const casadi::MX& controls, - casadi::MX& interpControls) const override; - // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(const casadi::MX& controlsVars, + casadi::MX& controls) const override; + std::vector> getVariableOrder() const override; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index f93be588d8..73703e65ff 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -16,6 +16,7 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ #include "CasOCLegendreGauss.h" +#include using casadi::DM; using casadi::MX; @@ -92,20 +93,40 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, } void LegendreGauss::calcInterpolatingControlsImpl( - const casadi::MX& controls, casadi::MX& interpControls) const { - if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + const casadi::MX& controlVars, casadi::MX& controls) const { + // if (m_problem.getNumControls() && + // m_solver.getInterpolateControlMidpoints()) { + // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + // const int igrid = imesh * (m_degree + 1); + // const auto c_i = controls(Slice(), igrid); + // const auto c_ip1 = controls(Slice(), igrid + m_degree + 1); + // for (int d = 0; d < m_degree; ++d) { + // const auto c_t = controls(Slice(), igrid + d + 1); + // interpControls(Slice(), imesh * m_degree + d) = + // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); + // } + // } + // } + + // Define the first control using backwards interpolation. + const auto c0 = controlVars(Slice(), 0); + const auto c1 = controlVars(Slice(), 1); + const double l0 = m_legendreRoots[0]; + const double l1 = m_legendreRoots[1]; + const casadi::MX m = (c1 - c0) / (l1 - l0); + const casadi::MX b = c0 - m * l0; + controls(Slice(), 0) = c0 - m * l0; + + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + for (int d = 0; d < m_degree; ++d) { const int igrid = imesh * (m_degree + 1); - const auto c_i = controls(Slice(), igrid); - const auto c_ip1 = controls(Slice(), igrid + m_degree + 1); - for (int d = 0; d < m_degree; ++d) { - const auto c_t = controls(Slice(), igrid + d + 1); - interpControls(Slice(), imesh * m_degree + d) = - c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); - } + controls(Slice(), igrid + d + 1) = + controlVars(Slice(), m_degree * imesh + d); } } + + + } std::vector> LegendreGauss::getVariableOrder() const { @@ -120,8 +141,14 @@ std::vector> LegendreGauss::getVariableOrder() const { for (int i = 1; i < N; ++i) { order.push_back({states, igrid + i}); } - for (int i = 0; i < N; ++i) { - order.push_back({controls, igrid + i}); + if (m_solver.getInterpolateControlMidpoints()) { + for (int d = 0; d < m_degree; ++d) { + order.push_back({controls, m_degree * imesh + d}); + } + } else { + for (int i = 0; i < N; ++i) { + order.push_back({controls, igrid + i}); + } } for (int i = 0; i < N; ++i) { order.push_back({multipliers, igrid + i}); @@ -135,27 +162,13 @@ std::vector> LegendreGauss::getVariableOrder() const { order.push_back({initial_time, m_numMeshIntervals}); order.push_back({final_time, m_numMeshIntervals}); order.push_back({parameters, m_numMeshIntervals}); - order.push_back({controls, m_numGridPoints - 1}); + if (!m_solver.getInterpolateControlMidpoints()) { + order.push_back({controls, m_numGridPoints - 1}); + } order.push_back({multipliers, m_numGridPoints - 1}); order.push_back({derivatives, m_numGridPoints - 1}); return order; } -// void LegendreGauss::calcExtrapolatedControlsImpl(casadi::MX& controls) const { -// if (m_problem.getNumControls()) { -// // TODO -// // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { -// // const int igrid = imesh * (m_degree + 1); -// // const auto c_i = controls(Slice(), igrid); -// // const auto c_ip1 = controls(Slice(), igrid + m_degree + 1); -// // for (int d = 0; d < m_degree; ++d) { -// // const auto c_t = controls(Slice(), igrid + d + 1); -// // interpControls(Slice(), imesh * m_degree + d) = -// // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); -// // } -// // } -// } -// } - } // namespace CasOC \ No newline at end of file diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index eee5cb8c88..9800e70777 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -71,12 +71,6 @@ class LegendreGauss : public Transcription { const int numMeshIntervals = (int)mesh.size() - 1; const int numGridPoints = (int)mesh.size() + numMeshIntervals * m_degree; casadi::DM grid = casadi::DM::zeros(1, numGridPoints); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); - casadi::DM pointsForInterpControls; - if (interpControls) { - pointsForInterpControls = casadi::DM::zeros(1, - numMeshIntervals * m_degree); - } // Get the collocation points (roots of Legendre polynomials). The roots // are returned on the interval (0, 1), not (-1, 1) as in the theses of @@ -88,28 +82,24 @@ class LegendreGauss : public Transcription { m_interpolationCoefficients, m_quadratureCoefficients); - // Create the grid points. - // std::vector controlPoints; + // Create the grid and control points. + std::vector controlPoints; for (int imesh = 0; imesh < numMeshIntervals; ++imesh) { const double t_i = mesh[imesh]; const double t_ip1 = mesh[imesh + 1]; int igrid = imesh * (m_degree + 1); grid(igrid) = t_i; - // controlPoints.push_back(false); + controlPoints.push_back(false); for (int d = 0; d < m_degree; ++d) { grid(igrid + d + 1) = t_i + (t_ip1 - t_i) * m_legendreRoots[d]; - // controlPoints.push_back(true); - if (interpControls) { - pointsForInterpControls(imesh * m_degree + d) = - grid(igrid + d + 1); - } + controlPoints.push_back(true); } } grid(numGridPoints - 1) = mesh[numMeshIntervals]; - // controlPoints.push_back(false); - createVariablesAndSetBounds(grid, + controlPoints.push_back(false); + createVariablesAndSetBounds(grid, (m_degree + 1) * m_problem.getNumStates(), m_degree + 2, - pointsForInterpControls); + controlPoints); } private: @@ -118,10 +108,9 @@ class LegendreGauss : public Transcription { void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(const casadi::MX& controls, - casadi::MX& interpControls) const override; + void calcInterpolatingControlsImpl(const casadi::MX& controlsVars, + casadi::MX& controls) const override; std::vector> getVariableOrder() const override; - // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; int m_degree; std::vector m_legendreRoots; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 7bca13634f..e0f9ebdee3 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -101,21 +101,46 @@ void LegendreGaussRadau::calcInterpolatingControlsImpl( } } -// void LegendreGaussRadau::calcExtrapolatedControlsImpl( -// casadi::MX& controls) const { -// if (m_problem.getNumControls()) { -// // TODO -// // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { -// // const int igrid = imesh * m_degree; -// // const auto c_i = controls(Slice(), igrid); -// // const auto c_ip1 = controls(Slice(), igrid + m_degree); -// // for (int d = 0; d < m_degree-1; ++d) { -// // const auto c_t = controls(Slice(), igrid + d + 1); -// // interpControls(Slice(), imesh * (m_degree - 1) + d) = -// // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); -// // } -// // } -// } -// } +std::vector> LegendreGaussRadau::getVariableOrder() const { + std::vector> order; + int N = m_numPointsPerMeshInterval - 1; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + int igrid = imesh * N; + order.push_back({states, igrid}); + order.push_back({initial_time, imesh}); + order.push_back({final_time, imesh}); + order.push_back({parameters, imesh}); + for (int i = 1; i < N; ++i) { + order.push_back({states, igrid + i}); + } + if (m_solver.getInterpolateControlMidpoints()) { + for (int d = 0; d < m_degree; ++d) { + order.push_back({controls, m_degree * imesh + d}); + } + } else { + for (int i = 0; i < N; ++i) { + order.push_back({controls, igrid + i}); + } + } + for (int i = 0; i < N; ++i) { + order.push_back({multipliers, igrid + i}); + } + for (int i = 0; i < N; ++i) { + order.push_back({derivatives, igrid + i}); + } + order.push_back({slacks, imesh}); + } + order.push_back({states, m_numGridPoints - 1}); + order.push_back({initial_time, m_numMeshIntervals}); + order.push_back({final_time, m_numMeshIntervals}); + order.push_back({parameters, m_numMeshIntervals}); + if (!m_solver.getInterpolateControlMidpoints()) { + order.push_back({controls, m_numGridPoints - 1}); + } + order.push_back({multipliers, m_numGridPoints - 1}); + order.push_back({derivatives, m_numGridPoints - 1}); + + return order; +} } // namespace CasOC \ No newline at end of file diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 446ba97219..3c13647534 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -106,16 +106,15 @@ class LegendreGaussRadau : public Transcription { } grid(numGridPoints - 1) = mesh[numMeshIntervals]; - // std::vector controlPoints; - // controlPoints.push_back(false); - // for (int i = 0; i < numGridPoints - 1; ++i) { - // controlPoints.push_back(true); - // } + // Create the control points. + std::vector controlPoints; + controlPoints.push_back(false); + for (int i = 0; i < numGridPoints - 1; ++i) { + controlPoints.push_back(true); + } - createVariablesAndSetBounds(grid, - m_degree * m_problem.getNumStates(), - m_degree + 1, - pointsForInterpControls); + createVariablesAndSetBounds(grid, m_degree * m_problem.getNumStates(), + m_degree + 1, controlPoints); } private: @@ -124,9 +123,9 @@ class LegendreGaussRadau : public Transcription { void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(const casadi::MX& controls, - casadi::MX& interpControls) const override; - // void calcExtrapolatedControlsImpl(casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(const casadi::MX& controlVars, + casadi::MX& controls) const override; + std::vector> getVariableOrder() const override; int m_degree; std::vector m_legendreRoots; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 7f2f392c37..d5ef35993f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -84,8 +84,7 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, int numPointsPerMeshInterval, - const casadi::DM& pointsForInterpControls) { - // const std::vector& controlPoints) { + const std::vector& controlPoints) { // Set the grid. // ------------- @@ -101,20 +100,19 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numDefectsPerMeshInterval = numDefectsPerMeshInterval + m_problem.getNumParameters() + 2; m_numPointsPerMeshInterval = numPointsPerMeshInterval; - m_pointsForInterpControls = pointsForInterpControls; - // m_controlPoints = controlPoints; + m_controlPoints = controlPoints; + m_numControlPoints = static_cast( + std::count(m_controlPoints.begin(), m_controlPoints.end(), true)); m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() : 0; m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); m_numConstraints = - // (m_problem.getNumParameters() + 2) * m_numMeshIntervals + m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + - m_problem.getNumKinematicConstraintEquations() * m_numMeshPoints + - m_problem.getNumControls() * (int)pointsForInterpControls.numel(); + m_problem.getNumKinematicConstraintEquations() * m_numMeshPoints; m_constraints.endpoint.resize( m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { @@ -132,12 +130,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Create variables. // ----------------- - // OPENSIM_THROW_IF( - // static_cast(m_controlPoints.size()) != m_numGridPoints, - // OpenSim::Exception, - // "The number of control points must match the number of grid " - // "points."); - + // Parameter variables. for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { m_scaledVectorVars[initial_time].push_back( MX::sym("initial_time_" + std::to_string(imesh), 1, 1)); @@ -147,23 +140,23 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, MX::sym("parameters_" + std::to_string(imesh), m_problem.getNumParameters(), 1)); } - + // Trajectory variables. for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { m_scaledVectorVars[states].push_back( MX::sym("states_" + std::to_string(igrid), m_problem.getNumStates(), 1)); - m_scaledVectorVars[controls].push_back( + + if (m_solver.getInterpolateControlMidpoints() && + m_controlPoints[igrid]) { + m_scaledVectorVars[controls].push_back( + MX::sym("controls_" + std::to_string(igrid), + m_problem.getNumControls(), 1)); + } else { + m_scaledVectorVars[controls].push_back( MX::sym("controls_" + std::to_string(igrid), m_problem.getNumControls(), 1)); - // if (m_controlPoints[igrid]) { - // m_scaledVectorVars[controls].push_back( - // MX::sym("controls_" + std::to_string(igrid), - // m_problem.getNumControls(), 1)); - // } else { - // m_scaledVectorVars[controls].push_back( - // MX(m_problem.getNumControls(), 1)); - // } - // TODO should multipliers and derivatives also follow controlPoints? + } + m_scaledVectorVars[multipliers].push_back( MX::sym("multipliers_" + std::to_string(igrid), m_problem.getNumMultipliers(), 1)); @@ -268,8 +261,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, const auto& controlInfos = m_problem.getControlInfos(); int ic = 0; for (const auto& info : controlInfos) { - setVariableBounds( - controls, ic, Slice(1, m_numGridPoints - 1), info.bounds); + setVariableBounds(controls, ic, Slice(1, m_numControlPoints - 1), + info.bounds); setVariableBounds(controls, ic, 0, info.initialBounds); setVariableBounds(controls, ic, -1, info.finalBounds); setVariableScaling(controls, Slice(), Slice(), info.bounds); @@ -329,6 +322,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } m_unscaledVars = unscaleVariables(m_scaledVars); + m_controls = MX(casadi::Sparsity::dense( + m_problem.getNumControls(), m_numGridPoints)); + calcInterpolatingControls(); + m_times = MX(casadi::Sparsity::dense(1, m_numGridPoints)); m_parameters = MX(casadi::Sparsity::dense( m_problem.getNumParameters(), m_numGridPoints)); @@ -352,8 +349,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; m_intervals = m_duration / m_numMeshIntervals; - - // calcExtrapolatedControls(); } void Transcription::transcribe() { @@ -539,18 +534,6 @@ void Transcription::transcribe() { casadi::DM::repmat(info.upperBounds, 1, m_numPathConstraintPoints); } - - // Interpolating controls. - // ----------------------- - m_constraints.interp_controls = - casadi::DM(casadi::Sparsity::dense(m_problem.getNumControls(), - (int)m_pointsForInterpControls.numel())); - const auto boundsOnInterpControls = casadi::DM::zeros( - m_problem.getNumControls(), (int)m_pointsForInterpControls.numel()); - m_constraintsLowerBounds.interp_controls = boundsOnInterpControls; - m_constraintsUpperBounds.interp_controls = boundsOnInterpControls; - - calcInterpolatingControls(); } void Transcription::setObjectiveAndEndpointConstraints() { @@ -607,12 +590,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { info.endpoint_function->call( {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), - m_unscaledVars[controls](Slice(), 0), + m_controls(Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), m_unscaledVars[final_time](0), m_unscaledVars[states](Slice(), -1), - m_unscaledVars[controls](Slice(), -1), + m_controls(Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), m_unscaledVars[parameters](Slice(), -1), @@ -681,12 +664,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { info.endpoint_function->call( {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), - m_unscaledVars[controls](Slice(), 0), + m_controls(Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), m_unscaledVars[final_time](0), m_unscaledVars[states](Slice(), -1), - m_unscaledVars[controls](Slice(), -1), + m_controls(Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), m_unscaledVars[parameters](Slice(), -1), @@ -959,6 +942,7 @@ void Transcription::printConstraintValues(const Iterate& it, const auto& upper = m_upperBounds; print_bounds("State bounds", it.state_names, it.times, vars.at(states), lower.at(states), upper.at(states)); + // TODO how to print bounds when controls are extrapolated? print_bounds("Control bounds", it.control_names, it.times, vars.at(controls), lower.at(controls), upper.at(controls)); print_bounds("Multiplier bounds", it.multiplier_names, it.times, diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 56f5e155ee..ad64d99db2 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -62,9 +62,6 @@ class Transcription { return meshIndices; } - casadi::DM repeatCopyParameter(const casadi::DM ¶meter) const { - return casadi::DM::repmat(parameter, 1, m_numMeshIntervals); - } Solution solve(const Iterate& guessOrig); @@ -77,8 +74,7 @@ class Transcription { void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, int numPointsPerMeshInterval, - const casadi::DM& pointsForInterpControls = casadi::DM()); - // const std::vector& controlPoints); + const std::vector& controlPoints); /// We assume all functions depend on time and parameters. /// "inputs" is prepended by time and postpended (?) by parameters. @@ -134,7 +130,6 @@ class Transcription { T kinematic; std::vector endpoint; std::vector path; - T interp_controls; }; void printConstraintValues(const Iterate& it, const Constraints& constraints, @@ -151,19 +146,19 @@ class Transcription { int m_numMeshInteriorPoints = -1; int m_numDefectsPerMeshInterval = -1; int m_numPointsPerMeshInterval = -1; + int m_numControlPoints = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; int m_numParameterConstraints = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; casadi::DM m_grid; - casadi::DM m_pointsForInterpControls; casadi::MX m_times; casadi::MX m_parameters; casadi::MX m_intervals; casadi::MX m_duration; - std::vector m_controlPoints; casadi::MX m_controls; + std::vector m_controlPoints; private: VectorVariablesMX m_scaledVectorVars; @@ -204,56 +199,14 @@ class Transcription { virtual void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const = 0; - virtual void calcInterpolatingControlsImpl(const casadi::MX& /*controls*/, - casadi::MX& /*interpControls*/) const { - OPENSIM_THROW_IF(m_pointsForInterpControls.numel(), OpenSim::Exception, - "Must provide constraints for interpolating controls.") - } - // virtual void calcExtrapolatedControlsImpl(casadi::MX& /*controls*/) const { - // bool hasExtrapolatedControls = std::any_of( - // m_controlPoints.begin(), m_controlPoints.end(), - // [](bool b) { return !b; }); - // OPENSIM_THROW_IF(hasExtrapolatedControls, OpenSim::Exception, - // "Must provide scheme for extrapolated controls.") - // } - - /// Use this function to ensure you iterate through variables in the same - /// order. Returns a vector whose elements are pairs of variable keys and - /// trajectory indexes. The vector should have length equal to the length of - /// the flattened variable vector passed to nlpsol(). This may be overridden - /// in derived classes to provide scheme-specific sparsity patterns. - virtual std::vector> getVariableOrder() const { - std::vector> order; - int N = m_numPointsPerMeshInterval - 1; - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - int igrid = imesh * N; - order.push_back({initial_time, imesh}); - order.push_back({final_time, imesh}); - order.push_back({parameters, imesh}); - for (int i = 0; i < N; ++i) { - order.push_back({states, igrid + i}); - } - for (int i = 0; i < N; ++i) { - order.push_back({controls, igrid + i}); - } - for (int i = 0; i < N; ++i) { - order.push_back({multipliers, igrid + i}); - } - for (int i = 0; i < N; ++i) { - order.push_back({derivatives, igrid + i}); - } - order.push_back({slacks, imesh}); - } - order.push_back({initial_time, m_numMeshIntervals}); - order.push_back({final_time, m_numMeshIntervals}); - order.push_back({parameters, m_numMeshIntervals}); - order.push_back({states, m_numGridPoints - 1}); - order.push_back({controls, m_numGridPoints - 1}); - order.push_back({multipliers, m_numGridPoints - 1}); - order.push_back({derivatives, m_numGridPoints - 1}); - - return order; - } + /// Override this function in your derived class to interpolate controls + /// for time points where control variables are not defined. + virtual void calcInterpolatingControlsImpl(const casadi::MX& controlVars, + casadi::MX& controls) const = 0; + /// Override this function to define the order of variables in the flattened + /// variable vector passed to nlpsol(). Returns a vector whose elements are + /// pairs of variable keys and trajectory indexes. + virtual std::vector> getVariableOrder() const = 0; void transcribe(); void setObjectiveAndEndpointConstraints(); @@ -263,12 +216,13 @@ class Transcription { m_unscaledVars.at(parameters), m_constraints.defects); } void calcInterpolatingControls() { - calcInterpolatingControlsImpl( - m_unscaledVars.at(controls), m_constraints.interp_controls); + if (m_solver.getInterpolateControlMidpoints()) { + calcInterpolatingControlsImpl( + m_unscaledVars.at(controls), m_controls); + } else { + m_controls = m_unscaledVars.at(controls); + } } - // void calcExtrapolatedControls() { - // calcExtrapolatedControlsImpl(m_scaledVars.at(controls)); - // } /// Convert the map of variables into a column vector, for passing onto /// nlpsol(), etc. @@ -460,13 +414,6 @@ class Transcription { copyColumn(path, imesh); } } - - // Interpolating controls. - if (m_pointsForInterpControls.numel()) { - for (int i = 0; i < N-1; ++i) { - copyColumn(constraints.interp_controls, icon++); - } - } } // Final grid point. @@ -516,8 +463,6 @@ class Transcription { const auto& info = m_problem.getPathConstraintInfos()[ipc]; out.path[ipc] = init(info.size(), m_numPathConstraintPoints); } - out.interp_controls = init(m_problem.getNumControls(), - (int)m_pointsForInterpControls.numel()); int iflat = 0; auto copyColumn = [&flat, &iflat](T& matrix, int columnIndex) { @@ -563,13 +508,6 @@ class Transcription { copyColumn(path, imesh); } } - - // Interpolating controls. - if (m_pointsForInterpControls.numel()) { - for (int i = 0; i < N-1; ++i) { - copyColumn(out.interp_controls, icon++); - } - } } // Final grid point. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index fc9954a516..e505300aef 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -70,6 +70,13 @@ void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, } } +void Trapezoidal::calcInterpolatingControlsImpl(const casadi::MX& controlVars, + casadi::MX& controls) const { + // For trapezoidal transcription, the control variables require no + // interpolation. + controls = controlVars; +} + std::vector> Trapezoidal::getVariableOrder() const { std::vector> order; for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index 512fde4aff..d78abe0ff7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -36,12 +36,12 @@ class Trapezoidal : public Transcription { "not supported with trapezoidal transcription."); const auto& mesh = m_solver.getMesh(); - // std::vector controlPoints; - // for (int i = 0; i < static_cast(mesh.size()); ++i) { - // controlPoints.push_back(true); - // } - createVariablesAndSetBounds(mesh, - m_problem.getNumStates(), 2); + std::vector controlPoints; + for (int i = 0; i < static_cast(mesh.size()); ++i) { + controlPoints.push_back(true); + } + createVariablesAndSetBounds(mesh, m_problem.getNumStates(), 2, + controlPoints); } private: @@ -51,6 +51,8 @@ class Trapezoidal : public Transcription { void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; + void calcInterpolatingControlsImpl(const casadi::MX& controlVars, + casadi::MX& controls) const override; std::vector> getVariableOrder() const override; }; From d073e49db8ed1c7dd21b6cdcf8a0630665e09e0a Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 12 Aug 2024 16:54:24 -0700 Subject: [PATCH 16/38] Problem solving with interpolated controls! --- .../exampleSlidingMass/exampleSlidingMass.cpp | 20 +-- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 24 ++-- .../MocoCasADiSolver/CasOCHermiteSimpson.h | 10 +- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 75 ++++++----- .../MocoCasADiSolver/CasOCLegendreGauss.h | 15 +-- .../CasOCLegendreGaussRadau.cpp | 34 +++-- .../CasOCLegendreGaussRadau.h | 13 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 120 ++++++++++-------- .../MocoCasADiSolver/CasOCTranscription.h | 33 +++-- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 10 +- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 14 +- 11 files changed, 193 insertions(+), 175 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 77f837e908..4fbbebab6e 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -53,11 +53,11 @@ std::unique_ptr createSlidingMassModel() { coord.setName("position"); model->addComponent(joint); - // auto* actu = new CoordinateActuator(); - // actu->setCoordinate(&coord); - // actu->setName("actuator"); - // actu->setOptimalForce(1); - // model->addComponent(actu); + auto* actu = new CoordinateActuator(); + actu->setCoordinate(&coord); + actu->setName("actuator"); + actu->setOptimalForce(1); + model->addComponent(actu); body->attachGeometry(new Sphere(0.05)); @@ -90,23 +90,23 @@ int main() { MocoInitialBounds(0), MocoFinalBounds(1)); // Speed must be within [-50, 50] throughout the motion. // Initial and final speed must be 0. Use compact syntax. - problem.setStateInfo("/slider/position/speed", {-50, 50}); + problem.setStateInfo("/slider/position/speed", {-50, 50}, 0, 0); // Applied force must be between -50 and 50. - // problem.setControlInfo("/actuator", MocoBounds(-50, 50)); + problem.setControlInfo("/actuator", MocoBounds(-50, 50)); // Cost. // ----- - // problem.addGoal(); + problem.addGoal(); // Configure the solver. // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); - // solver.set_parallel(0); + solver.set_parallel(0); solver.set_optim_solver("ipopt"); solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("legendre-gauss-3"); + solver.set_transcription_scheme("hermite-simpson"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 2ecc3e6f49..a20bfd3156 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -50,6 +50,12 @@ DM HermiteSimpson::createMeshIndicesImpl() const { return indices; } +DM HermiteSimpson::createControlIndicesImpl() const { + DM indices = DM::zeros(1, m_numGridPoints); + for (int i = 0; i < m_numGridPoints; i += 2) { indices(i) = 1; } + return indices; +} + void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const { @@ -94,15 +100,11 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, } } -void HermiteSimpson::calcInterpolatingControlsImpl( - const casadi::MX& controlVars, casadi::MX& controls) const { - +void HermiteSimpson::calcInterpolatingControlsImpl(casadi::MX& controls) const { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - controls(Slice(), 2 * imesh) = controlVars(Slice(), imesh); - controls(Slice(), 2 * imesh + 1) = 0.5 * ( - controlVars(Slice(), imesh + 1) + controlVars(Slice(), imesh)); + controls(Slice(), 2*imesh + 1) = 0.5 * ( + controls(Slice(), 2*imesh) + controls(Slice(), 2*imesh + 2)); } - controls(Slice(), -1) = controlVars(Slice(), -1); } std::vector> HermiteSimpson::getVariableOrder() const { @@ -116,15 +118,11 @@ std::vector> HermiteSimpson::getVariableOrder() const { order.push_back({states, igrid}); order.push_back({states, igrid + 1}); if (m_solver.getInterpolateControlMidpoints()) { - order.push_back({controls, imesh}); + order.push_back({controls, igrid}); } else { order.push_back({controls, igrid}); order.push_back({controls, igrid + 1}); } - - for (int i = 0; i < N; ++i) { - order.push_back({controls, igrid + i}); - } for (int i = 0; i < N; ++i) { order.push_back({multipliers, igrid + i}); } @@ -137,7 +135,7 @@ std::vector> HermiteSimpson::getVariableOrder() const { order.push_back({final_time, m_numMeshIntervals}); order.push_back({parameters, m_numMeshIntervals}); order.push_back({states, m_numGridPoints - 1}); - order.push_back({controls, m_numControlPoints - 1}); + order.push_back({controls, m_numGridPoints - 1}); order.push_back({multipliers, m_numGridPoints - 1}); order.push_back({derivatives, m_numGridPoints - 1}); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index e697534ec0..27e850826a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -47,28 +47,24 @@ class HermiteSimpson : public Transcription { casadi::DM grid = casadi::DM::zeros(1, (2 * m_solver.getMesh().size()) - 1); const auto& mesh = m_solver.getMesh(); - std::vector controlPoints; for (int i = 0; i < grid.numel(); ++i) { if (i % 2 == 0) { grid(i) = mesh[i / 2]; - controlPoints.push_back(true); } else { grid(i) = .5 * (mesh[i / 2] + mesh[i / 2 + 1]); - controlPoints.push_back(false); } } - createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), 3, - controlPoints); + createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), 3); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; + casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(const casadi::MX& controlsVars, - casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(casadi::MX& controls) const override; std::vector> getVariableOrder() const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 73703e65ff..e0d9aea73a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -55,6 +55,17 @@ DM LegendreGauss::createMeshIndicesImpl() const { return indices; } +DM LegendreGauss::createControlIndicesImpl() const { + DM indices = DM::zeros(1, m_numGridPoints); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + const int igrid = imesh * (m_degree + 1); + for (int d = 0; d < m_degree; ++d) { + indices(igrid + d + 1) = 1; + } + } + return indices; +} + void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const { @@ -63,12 +74,12 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const int NS = m_problem.getNumStates(); const int NP = m_problem.getNumParameters(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - const int i = imesh * (m_degree + 1); - const int ip1 = i + m_degree + 1; + const int igrid = imesh * (m_degree + 1); const auto h = m_intervals(imesh); - const auto x_i = x(Slice(), Slice(i, ip1)); - const auto xdot_i = xdot(Slice(), Slice(i + 1, ip1)); - const auto x_ip1 = x(Slice(), ip1); + const auto x_i = x(Slice(), Slice(igrid, igrid + m_degree + 1)); + const auto xdot_i = + xdot(Slice(), Slice(igrid + 1, igrid + m_degree + 1)); + const auto x_ip1 = x(Slice(), igrid + m_degree + 1); // End state interpolation. defects(Slice(0, NS), imesh) = @@ -92,41 +103,37 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, } } -void LegendreGauss::calcInterpolatingControlsImpl( - const casadi::MX& controlVars, casadi::MX& controls) const { - // if (m_problem.getNumControls() && - // m_solver.getInterpolateControlMidpoints()) { - // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - // const int igrid = imesh * (m_degree + 1); - // const auto c_i = controls(Slice(), igrid); - // const auto c_ip1 = controls(Slice(), igrid + m_degree + 1); - // for (int d = 0; d < m_degree; ++d) { - // const auto c_t = controls(Slice(), igrid + d + 1); - // interpControls(Slice(), imesh * m_degree + d) = - // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); - // } - // } - // } - - // Define the first control using backwards interpolation. - const auto c0 = controlVars(Slice(), 0); - const auto c1 = controlVars(Slice(), 1); - const double l0 = m_legendreRoots[0]; - const double l1 = m_legendreRoots[1]; - const casadi::MX m = (c1 - c0) / (l1 - l0); - const casadi::MX b = c0 - m * l0; - controls(Slice(), 0) = c0 - m * l0; +void LegendreGauss::calcInterpolatingControlsImpl(casadi::MX& controls) const { + + auto getLagrangePolynomial = [this](int i, double tau) -> double { + double polynomial = 1.0; + for (int d = 0; d < m_degree; ++d) { + if (i != d) { + polynomial *= (tau - m_legendreRoots[d]) / + (m_legendreRoots[i] - m_legendreRoots[d]); + } + } + return polynomial; + }; + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + const int igrid = imesh * (m_degree + 1); + controls(Slice(), igrid) = 0; for (int d = 0; d < m_degree; ++d) { - const int igrid = imesh * (m_degree + 1); - controls(Slice(), igrid + d + 1) = - controlVars(Slice(), m_degree * imesh + d); + const auto c_t = controls(Slice(), igrid + d + 1); + controls(Slice(), igrid) += getLagrangePolynomial(d, 0) * c_t; } } - - + // Final control interpolation. + controls(Slice(), m_numGridPoints - 1) = 0; + for (int d = 0; d < m_degree; ++d) { + const int igrid = (m_numMeshIntervals - 1) * (m_degree + 1); + const auto c_t = controls(Slice(), igrid); + controls(Slice(), m_numGridPoints - 1) += + getLagrangePolynomial(d, 1) * c_t; + } } std::vector> LegendreGauss::getVariableOrder() const { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 9800e70777..ad83c75c77 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -82,34 +82,29 @@ class LegendreGauss : public Transcription { m_interpolationCoefficients, m_quadratureCoefficients); - // Create the grid and control points. - std::vector controlPoints; + // Create the grid points. for (int imesh = 0; imesh < numMeshIntervals; ++imesh) { const double t_i = mesh[imesh]; const double t_ip1 = mesh[imesh + 1]; int igrid = imesh * (m_degree + 1); grid(igrid) = t_i; - controlPoints.push_back(false); for (int d = 0; d < m_degree; ++d) { grid(igrid + d + 1) = t_i + (t_ip1 - t_i) * m_legendreRoots[d]; - controlPoints.push_back(true); } } grid(numGridPoints - 1) = mesh[numMeshIntervals]; - controlPoints.push_back(false); - createVariablesAndSetBounds(grid, - (m_degree + 1) * m_problem.getNumStates(), m_degree + 2, - controlPoints); + createVariablesAndSetBounds(grid, + (m_degree + 1) * m_problem.getNumStates(), m_degree + 2); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; + casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(const casadi::MX& controlsVars, - casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(casadi::MX& controls) const override; std::vector> getVariableOrder() const override; int m_degree; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index e0f9ebdee3..f25fb6a67c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -52,6 +52,12 @@ DM LegendreGaussRadau::createMeshIndicesImpl() const { return indices; } +DM LegendreGaussRadau::createControlIndicesImpl() const { + DM indices = DM::ones(1, m_numGridPoints); + indices(0) = 0; + return indices; +} + void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const { @@ -85,20 +91,20 @@ void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, } void LegendreGaussRadau::calcInterpolatingControlsImpl( - const casadi::MX& controls, casadi::MX& interpControls) const { - if (m_problem.getNumControls() && - m_solver.getInterpolateControlMidpoints()) { - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - const int igrid = imesh * m_degree; - const auto c_i = controls(Slice(), igrid); - const auto c_ip1 = controls(Slice(), igrid + m_degree); - for (int d = 0; d < m_degree-1; ++d) { - const auto c_t = controls(Slice(), igrid + d + 1); - interpControls(Slice(), imesh * (m_degree - 1) + d) = - c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); - } - } - } + casadi::MX& controls) const { + // if (m_problem.getNumControls() && + // m_solver.getInterpolateControlMidpoints()) { + // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + // const int igrid = imesh * m_degree; + // const auto c_i = controls(Slice(), igrid); + // const auto c_ip1 = controls(Slice(), igrid + m_degree); + // for (int d = 0; d < m_degree-1; ++d) { + // const auto c_t = controls(Slice(), igrid + d + 1); + // interpControls(Slice(), imesh * (m_degree - 1) + d) = + // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); + // } + // } + // } } std::vector> LegendreGaussRadau::getVariableOrder() const { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 3c13647534..f1cd35d763 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -106,25 +106,18 @@ class LegendreGaussRadau : public Transcription { } grid(numGridPoints - 1) = mesh[numMeshIntervals]; - // Create the control points. - std::vector controlPoints; - controlPoints.push_back(false); - for (int i = 0; i < numGridPoints - 1; ++i) { - controlPoints.push_back(true); - } - createVariablesAndSetBounds(grid, m_degree * m_problem.getNumStates(), - m_degree + 1, controlPoints); + m_degree + 1); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; + casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(const casadi::MX& controlVars, - casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(casadi::MX& controls) const override; std::vector> getVariableOrder() const override; int m_degree; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index d5ef35993f..2e30196338 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -83,8 +83,7 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, - int numPointsPerMeshInterval, - const std::vector& controlPoints) { + int numPointsPerMeshInterval) { // Set the grid. // ------------- @@ -100,9 +99,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numDefectsPerMeshInterval = numDefectsPerMeshInterval + m_problem.getNumParameters() + 2; m_numPointsPerMeshInterval = numPointsPerMeshInterval; - m_controlPoints = controlPoints; - m_numControlPoints = static_cast( - std::count(m_controlPoints.begin(), m_controlPoints.end(), true)); m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() : 0; @@ -112,7 +108,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + m_numAuxiliaryResiduals * m_numGridPoints + - m_problem.getNumKinematicConstraintEquations() * m_numMeshPoints; + m_problem.getNumKinematicConstraintEquations() * m_numMeshPoints; m_constraints.endpoint.resize( m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { @@ -128,6 +124,45 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } m_grid = grid; + // Define indices. + // --------------- + m_meshIndicesMap = createMeshIndices(); + std::vector meshIndicesVector; + std::vector meshInteriorIndicesVector; + for (int i = 0; i < m_meshIndicesMap.size2(); ++i) { + if (m_meshIndicesMap(i).scalar() == 1) { + meshIndicesVector.push_back(i); + } else { + meshInteriorIndicesVector.push_back(i); + } + } + + auto controlIndicesMap = createControlIndices(); + std::vector controlIndicesVector; + for (int i = 0; i < controlIndicesMap.size2(); ++i) { + if (controlIndicesMap(i).scalar() == 1) { + controlIndicesVector.push_back(i); + } + } + m_numControlPoints = static_cast(controlIndicesVector.size()); + + auto makeTimeIndices = [](const std::vector& in) { + casadi::Matrix out(1, in.size()); + for (int i = 0; i < (int)in.size(); ++i) { out(i) = in[i]; } + return out; + }; + + std::vector gridIndicesVector(m_numGridPoints); + std::iota(gridIndicesVector.begin(), gridIndicesVector.end(), 0); + m_gridIndices = makeTimeIndices(gridIndicesVector); + m_meshIndices = makeTimeIndices(meshIndicesVector); + m_meshInteriorIndices = + makeTimeIndices(meshInteriorIndicesVector); + m_pathConstraintIndices = m_solver.getEnforcePathConstraintMidpoints() + ? makeTimeIndices(gridIndicesVector) + : makeTimeIndices(meshIndicesVector); + m_controlIndices = makeTimeIndices(controlIndicesVector); + // Create variables. // ----------------- // Parameter variables. @@ -140,23 +175,29 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, MX::sym("parameters_" + std::to_string(imesh), m_problem.getNumParameters(), 1)); } - // Trajectory variables. + // Trajectory variables. for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { m_scaledVectorVars[states].push_back( MX::sym("states_" + std::to_string(igrid), m_problem.getNumStates(), 1)); - if (m_solver.getInterpolateControlMidpoints() && - m_controlPoints[igrid]) { - m_scaledVectorVars[controls].push_back( - MX::sym("controls_" + std::to_string(igrid), - m_problem.getNumControls(), 1)); + if (m_solver.getInterpolateControlMidpoints()) { + auto it = std::find(controlIndicesVector.begin(), + controlIndicesVector.end(), igrid); + if (it != controlIndicesVector.end()) { + m_scaledVectorVars[controls].push_back( + MX::sym("controls_" + std::to_string(igrid), + m_problem.getNumControls(), 1)); + } else { + m_scaledVectorVars[controls].push_back( + MX(m_problem.getNumControls(), 1)); + } } else { m_scaledVectorVars[controls].push_back( MX::sym("controls_" + std::to_string(igrid), m_problem.getNumControls(), 1)); } - + m_scaledVectorVars[multipliers].push_back( MX::sym("multipliers_" + std::to_string(igrid), m_problem.getNumMultipliers(), 1)); @@ -184,32 +225,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVars[derivatives] = MX::horzcat(m_scaledVectorVars[derivatives]); m_scaledVars[slacks] = MX::horzcat(m_scaledVectorVars[slacks]); - m_meshIndicesMap = createMeshIndices(); - std::vector meshIndicesVector; - std::vector meshInteriorIndicesVector; - for (int i = 0; i < m_meshIndicesMap.size2(); ++i) { - if (m_meshIndicesMap(i).scalar() == 1) { - meshIndicesVector.push_back(i); - } else { - meshInteriorIndicesVector.push_back(i); - } - } - - auto makeTimeIndices = [](const std::vector& in) { - casadi::Matrix out(1, in.size()); - for (int i = 0; i < (int)in.size(); ++i) { out(i) = in[i]; } - return out; - }; - - std::vector gridIndicesVector(m_numGridPoints); - std::iota(gridIndicesVector.begin(), gridIndicesVector.end(), 0); - m_gridIndices = makeTimeIndices(gridIndicesVector); - m_meshIndices = makeTimeIndices(meshIndicesVector); - m_meshInteriorIndices = - makeTimeIndices(meshInteriorIndicesVector); - m_pathConstraintIndices = m_solver.getEnforcePathConstraintMidpoints() - ? makeTimeIndices(gridIndicesVector) - : makeTimeIndices(meshIndicesVector); + // Interplate controls. + calcInterpolatingControls(); // Set variable bounds. // -------------------- @@ -233,9 +250,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); - setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), + setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), {-casadi::inf, casadi::inf}); - setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), + setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), {-casadi::inf, casadi::inf}); setVariableScaling(initial_time, 0, Slice(), @@ -258,10 +275,11 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } { + // TODO update this to set bounds properly based on the control points. const auto& controlInfos = m_problem.getControlInfos(); int ic = 0; for (const auto& info : controlInfos) { - setVariableBounds(controls, ic, Slice(1, m_numControlPoints - 1), + setVariableBounds(controls, ic, Slice(1, m_numGridPoints - 1), info.bounds); setVariableBounds(controls, ic, 0, info.initialBounds); setVariableBounds(controls, ic, -1, info.finalBounds); @@ -322,10 +340,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } m_unscaledVars = unscaleVariables(m_scaledVars); - m_controls = MX(casadi::Sparsity::dense( - m_problem.getNumControls(), m_numGridPoints)); - calcInterpolatingControls(); - m_times = MX(casadi::Sparsity::dense(1, m_numGridPoints)); m_parameters = MX(casadi::Sparsity::dense( m_problem.getNumParameters(), m_numGridPoints)); @@ -590,12 +604,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { info.endpoint_function->call( {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), - m_controls(Slice(), 0), + m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), m_unscaledVars[final_time](0), m_unscaledVars[states](Slice(), -1), - m_controls(Slice(), -1), + m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), m_unscaledVars[parameters](Slice(), -1), @@ -617,7 +631,8 @@ void Transcription::setObjectiveAndEndpointConstraints() { // Minimize generalized accelerations. if (minimizeAccelerations) { const auto& numAccels = m_problem.getNumAccelerations(); - const auto accels = m_scaledVars[derivatives](Slice(0, numAccels), Slice()); + const auto accels = + m_scaledVars[derivatives](Slice(0, numAccels), Slice()); const double accelWeight = m_solver.getImplicitMultibodyAccelerationsWeight(); MX integrandTraj = MX::sum1(MX::sq(accels)); @@ -664,12 +679,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { info.endpoint_function->call( {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), - m_controls(Slice(), 0), + m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), m_unscaledVars[final_time](0), m_unscaledVars[states](Slice(), -1), - m_controls(Slice(), -1), + m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), m_unscaledVars[derivatives](Slice(), -1), m_unscaledVars[parameters](Slice(), -1), @@ -734,6 +749,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { } auto x = flattenVariables(m_scaledVectorVars); + std::cout << "variables: " << x << std::endl; casadi_int numVariables = x.numel(); // The m_constraints symbolic vector holds all of the expressions for @@ -1311,6 +1327,8 @@ casadi::MXVector Transcription::evalOnTrajectory( m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); } else if (inputs[i] == slacks) { mxIn[i + 1] = m_unscaledVars.at(inputs[i]); + // } else if (inputs[i] == controls) { + // mxIn[i + 1] = m_controls(Slice(), timeIndices); } else { mxIn[i + 1] = m_unscaledVars.at(inputs[i])(Slice(), timeIndices); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index ad64d99db2..03efc07ec3 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -62,6 +62,17 @@ class Transcription { return meshIndices; } + casadi::DM createControlIndices() const { + casadi::DM controlIndices = createControlIndicesImpl(); + const auto shape = controlIndices.size(); + OPENSIM_THROW_IF(shape.first != 1 || shape.second != m_numGridPoints, + OpenSim::Exception, + "createControlIndicesImpl() must return a row vector of shape " + "length [1, {}], but a matrix of shape [{}, {}] was returned.", + m_numGridPoints, shape.first, shape.second); + + return controlIndices; + } Solution solve(const Iterate& guessOrig); @@ -73,8 +84,7 @@ class Transcription { /// TODO control points void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, - int numPointsPerMeshInterval, - const std::vector& controlPoints); + int numPointsPerMeshInterval); /// We assume all functions depend on time and parameters. /// "inputs" is prepended by time and postpended (?) by parameters. @@ -157,8 +167,6 @@ class Transcription { casadi::MX m_parameters; casadi::MX m_intervals; casadi::MX m_duration; - casadi::MX m_controls; - std::vector m_controlPoints; private: VectorVariablesMX m_scaledVectorVars; @@ -174,6 +182,7 @@ class Transcription { casadi::Matrix m_meshIndices; casadi::Matrix m_meshInteriorIndices; casadi::Matrix m_pathConstraintIndices; + casadi::Matrix m_controlIndices; casadi::MX m_xdot; // State derivatives. @@ -194,17 +203,18 @@ class Transcription { /// @note The returned vector must be a row vector of length m_numGridPoints /// with nonzero values at the mesh indices. virtual casadi::DM createMeshIndicesImpl() const = 0; + // TODO + virtual casadi::DM createControlIndicesImpl() const = 0; /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. virtual void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const = 0; - /// Override this function in your derived class to interpolate controls + /// Override this function in your derived class to interpolate controls /// for time points where control variables are not defined. - virtual void calcInterpolatingControlsImpl(const casadi::MX& controlVars, - casadi::MX& controls) const = 0; + virtual void calcInterpolatingControlsImpl(casadi::MX& controls) const = 0; /// Override this function to define the order of variables in the flattened - /// variable vector passed to nlpsol(). Returns a vector whose elements are + /// variable vector passed to nlpsol(). Returns a vector whose elements are /// pairs of variable keys and trajectory indexes. virtual std::vector> getVariableOrder() const = 0; @@ -217,10 +227,7 @@ class Transcription { } void calcInterpolatingControls() { if (m_solver.getInterpolateControlMidpoints()) { - calcInterpolatingControlsImpl( - m_unscaledVars.at(controls), m_controls); - } else { - m_controls = m_unscaledVars.at(controls); + calcInterpolatingControlsImpl(m_scaledVars.at(controls)); } } @@ -235,6 +242,7 @@ class Transcription { return casadi::MX::vertcat(stdvec); } + /// Convert the map of variables into a column vector, for passing onto /// nlpsol(), etc. casadi::DM flattenVariables(const VariablesDM& vars) const { @@ -248,6 +256,7 @@ class Transcription { return casadi::DM::vertcat(stdvec); } + /// Convert the 'x' column vector into separate variables. VariablesDM expandVariables(const casadi::DM& x) const { VariablesDM out; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index e505300aef..b314b6605c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -40,6 +40,10 @@ DM Trapezoidal::createMeshIndicesImpl() const { return DM::ones(1, m_numGridPoints); } +DM Trapezoidal::createControlIndicesImpl() const { + return DM::ones(1, m_numGridPoints); +} + void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const { @@ -70,11 +74,9 @@ void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, } } -void Trapezoidal::calcInterpolatingControlsImpl(const casadi::MX& controlVars, - casadi::MX& controls) const { - // For trapezoidal transcription, the control variables require no +void Trapezoidal::calcInterpolatingControlsImpl(casadi::MX& controls) const { + // For trapezoidal transcription, the control variables require no // interpolation. - controls = controlVars; } std::vector> Trapezoidal::getVariableOrder() const { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index d78abe0ff7..9cb1511340 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -35,24 +35,18 @@ class Trapezoidal : public Transcription { "Enforcing kinematic constraint derivatives " "not supported with trapezoidal transcription."); - const auto& mesh = m_solver.getMesh(); - std::vector controlPoints; - for (int i = 0; i < static_cast(mesh.size()); ++i) { - controlPoints.push_back(true); - } - createVariablesAndSetBounds(mesh, m_problem.getNumStates(), 2, - controlPoints); + createVariablesAndSetBounds(m_solver.getMesh(), + m_problem.getNumStates(), 2); } private: casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; - + casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(const casadi::MX& controlVars, - casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(casadi::MX& controls) const override; std::vector> getVariableOrder() const override; }; From f7650cfe94927bd372648b53aae7e657835d9749 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 14 Aug 2024 16:07:02 -0700 Subject: [PATCH 17/38] Problem is working with controls (!) --- .../exampleSlidingMass/exampleSlidingMass.cpp | 2 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 9 ++-- .../MocoCasADiSolver/CasOCHermiteSimpson.h | 27 +++++++++++- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 35 +++------------ .../MocoCasADiSolver/CasOCLegendreGauss.h | 44 +++++++++++++++++++ .../CasOCLegendreGaussRadau.cpp | 21 +++------ .../CasOCLegendreGaussRadau.h | 40 ++++++++++++----- .../MocoCasADiSolver/CasOCTranscription.cpp | 4 +- .../MocoCasADiSolver/CasOCTranscription.h | 23 ++++++++-- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 5 --- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 1 - 11 files changed, 139 insertions(+), 72 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 4fbbebab6e..72f6df6d6c 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -106,7 +106,7 @@ int main() { solver.set_parallel(0); solver.set_optim_solver("ipopt"); solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("hermite-simpson"); + solver.set_transcription_scheme("legendre-gauss-radau-3"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index a20bfd3156..e04086185b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -101,10 +101,11 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MX& x, } void HermiteSimpson::calcInterpolatingControlsImpl(casadi::MX& controls) const { - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - controls(Slice(), 2*imesh + 1) = 0.5 * ( - controls(Slice(), 2*imesh) + controls(Slice(), 2*imesh + 2)); - } + calcInterpolatingControlsHelper(controls); +} + +void HermiteSimpson::calcInterpolatingControlsImpl(casadi::DM& controls) const { + calcInterpolatingControlsHelper(controls); } std::vector> HermiteSimpson::getVariableOrder() const { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 27e850826a..f175af1bd8 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -40,6 +40,16 @@ namespace CasOC { /// Kinematic constraint and path constraint errors are enforced only at the /// mesh points. Errors at collocation points at the mesh interval midpoint /// are ignored. +/// +/// References +/// ---------- +/// [1] Hargraves, C.R., Paris, S.W. "Direct Trajectory Optimization Using +/// Nonlinear Programming and Collocation." Journal of Guidance, Control, +/// and Dynamics (1987). +/// [2] Bordalba, Ricard, Tobias Schoels, Lluís Ros, Josep M. Porta, and +/// Moritz Diehl. "Direct collocation methods for trajectory optimization +/// in constrained robotic systems." IEEE Transactions on Robotics (2023). +/// class HermiteSimpson : public Transcription { public: HermiteSimpson(const Solver& solver, const Problem& problem) @@ -64,8 +74,23 @@ class HermiteSimpson : public Transcription { void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(casadi::MX& controls) const override; std::vector> getVariableOrder() const override; + void calcInterpolatingControlsImpl(casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(casadi::DM& controls) const override; + + template + void calcInterpolatingControlsHelper(T& controls) const { + using casadi::Slice; + + // This control approximation scheme is based on the control scheme + // proposed by Hargraves and Paris (1987) [1]. Linear interpolation of + // controls is also recommended by Bordalba et al. (2023) [2]. + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + controls(Slice(), 2*imesh + 1) = 0.5 * ( + controls(Slice(), 2*imesh) + + controls(Slice(), 2*imesh + 2)); + } + } }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index e0d9aea73a..94c48769e3 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -104,36 +104,11 @@ void LegendreGauss::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, } void LegendreGauss::calcInterpolatingControlsImpl(casadi::MX& controls) const { + calcInterpolatingControlsHelper(controls); +} - auto getLagrangePolynomial = [this](int i, double tau) -> double { - double polynomial = 1.0; - for (int d = 0; d < m_degree; ++d) { - if (i != d) { - polynomial *= (tau - m_legendreRoots[d]) / - (m_legendreRoots[i] - m_legendreRoots[d]); - } - } - return polynomial; - }; - - - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - const int igrid = imesh * (m_degree + 1); - controls(Slice(), igrid) = 0; - for (int d = 0; d < m_degree; ++d) { - const auto c_t = controls(Slice(), igrid + d + 1); - controls(Slice(), igrid) += getLagrangePolynomial(d, 0) * c_t; - } - } - - // Final control interpolation. - controls(Slice(), m_numGridPoints - 1) = 0; - for (int d = 0; d < m_degree; ++d) { - const int igrid = (m_numMeshIntervals - 1) * (m_degree + 1); - const auto c_t = controls(Slice(), igrid); - controls(Slice(), m_numGridPoints - 1) += - getLagrangePolynomial(d, 1) * c_t; - } +void LegendreGauss::calcInterpolatingControlsImpl(casadi::DM& controls) const { + calcInterpolatingControlsHelper(controls); } std::vector> LegendreGauss::getVariableOrder() const { @@ -150,7 +125,7 @@ std::vector> LegendreGauss::getVariableOrder() const { } if (m_solver.getInterpolateControlMidpoints()) { for (int d = 0; d < m_degree; ++d) { - order.push_back({controls, m_degree * imesh + d}); + order.push_back({controls, igrid + d + 1}); } } else { for (int i = 0; i < N; ++i) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index ad83c75c77..50b364329b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -105,6 +105,7 @@ class LegendreGauss : public Transcription { const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(casadi::DM& controls) const override; std::vector> getVariableOrder() const override; int m_degree; @@ -112,6 +113,49 @@ class LegendreGauss : public Transcription { casadi::DM m_differentiationMatrix; casadi::DM m_interpolationCoefficients; casadi::DM m_quadratureCoefficients; + + template + void calcInterpolatingControlsHelper(T& controls) const { + using casadi::Slice; + + // This interpolation scheme is based on control approximation defined + // by Eq. 6.21 in the thesis of Huntington [2]. We cannot linearly + // interpolate the control values at the mesh points (as recommended by + // Bordalba et al. [3]) because this would violate the sparsity + // structure required by the FATROP solver. + + // Evaluate the `i`th Lagrange polynomial at the point `tau`. + auto getLagrangePolynomial = [this](int i, double tau) -> double { + double polynomial = 1.0; + for (int d = 0; d < m_degree; ++d) { + if (i != d) { + polynomial *= (tau - m_legendreRoots[d]) / + (m_legendreRoots[i] - m_legendreRoots[d]); + } + } + return polynomial; + }; + + // Define controls for the initial mesh point for all mesh intervals. + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + const int igrid = imesh * (m_degree + 1); + controls(Slice(), igrid) = 0; + for (int d = 0; d < m_degree; ++d) { + const auto c_t = controls(Slice(), igrid + d + 1); + controls(Slice(), igrid) += getLagrangePolynomial(d, 0) * c_t; + } + } + + // Define the control at the final mesh point for the final mesh + // interval. + controls(Slice(), m_numGridPoints - 1) = 0; + for (int d = 0; d < m_degree; ++d) { + const int igrid = (m_numMeshIntervals - 1) * (m_degree + 1); + const auto c_t = controls(Slice(), igrid); + controls(Slice(), m_numGridPoints - 1) += + getLagrangePolynomial(d, 1) * c_t; + } + } }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index f25fb6a67c..b4b5fdcb03 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -91,20 +91,13 @@ void LegendreGaussRadau::calcDefectsImpl(const casadi::MX& x, } void LegendreGaussRadau::calcInterpolatingControlsImpl( - casadi::MX& controls) const { - // if (m_problem.getNumControls() && - // m_solver.getInterpolateControlMidpoints()) { - // for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - // const int igrid = imesh * m_degree; - // const auto c_i = controls(Slice(), igrid); - // const auto c_ip1 = controls(Slice(), igrid + m_degree); - // for (int d = 0; d < m_degree-1; ++d) { - // const auto c_t = controls(Slice(), igrid + d + 1); - // interpControls(Slice(), imesh * (m_degree - 1) + d) = - // c_t - (m_legendreRoots[d] * (c_ip1 - c_i) + c_i); - // } - // } - // } + casadi::MX& controls) const { + calcInterpolatingControlsHelper(controls); +} + +void LegendreGaussRadau::calcInterpolatingControlsImpl( + casadi::DM& controls) const { + calcInterpolatingControlsHelper(controls); } std::vector> LegendreGaussRadau::getVariableOrder() const { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index f1cd35d763..6193a6e937 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -71,12 +71,6 @@ class LegendreGaussRadau : public Transcription { const int numGridPoints = (int)mesh.size() + numMeshIntervals * (m_degree - 1); casadi::DM grid = casadi::DM::zeros(1, numGridPoints); - const bool interpControls = m_solver.getInterpolateControlMidpoints(); - casadi::DM pointsForInterpControls; - if (interpControls) { - pointsForInterpControls = casadi::DM::zeros(1, - numMeshIntervals * (m_degree - 1)); - } // Get the collocation points (roots of Legendre polynomials). The roots // are returned on the interval (0, 1], not (-1, 1] as in the theses of @@ -98,10 +92,6 @@ class LegendreGaussRadau : public Transcription { grid(igrid + m_degree) = t_ip1; for (int d = 0; d < m_degree-1; ++d) { grid(igrid + d + 1) = t_i + (t_ip1 - t_i) * m_legendreRoots[d]; - if (interpControls) { - pointsForInterpControls(imesh * (m_degree - 1) + d) = - grid(igrid + d + 1); - } } } grid(numGridPoints - 1) = mesh[numMeshIntervals]; @@ -118,6 +108,7 @@ class LegendreGaussRadau : public Transcription { const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; + void calcInterpolatingControlsImpl(casadi::DM& controls) const override; std::vector> getVariableOrder() const override; int m_degree; @@ -125,6 +116,35 @@ class LegendreGaussRadau : public Transcription { casadi::DM m_differentiationMatrix; casadi::DM m_interpolationCoefficients; casadi::DM m_quadratureCoefficients; + + template + void calcInterpolatingControlsHelper(T& controls) const { + using casadi::Slice; + + // This interpolation scheme is based on control approximation defined + // by Eq. 6.16 in the thesis of Huntington [2]. + + // Evaluate the `i`th Lagrange polynomial at the point `tau`. + auto getLagrangePolynomial = [this](int i, double tau) -> double { + double polynomial = 1.0; + for (int d = 0; d < m_degree; ++d) { + if (i != d) { + polynomial *= (tau - m_legendreRoots[d]) / + (m_legendreRoots[i] - m_legendreRoots[d]); + } + } + return polynomial; + }; + + // The Legendre-Gauss-Radau points include a collocation point at the + // final mesh point for all mesh intervals. Therefore, we only need to + // interpolate the control at the first mesh point. + controls(Slice(), 0) = 0; + for (int d = 0; d < m_degree; ++d) { + const auto c_t = controls(Slice(), d + 1); + controls(Slice(), 0) += getLagrangePolynomial(d, 0) * c_t; + } + } }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 2e30196338..9eb6373296 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -226,7 +226,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVars[slacks] = MX::horzcat(m_scaledVectorVars[slacks]); // Interplate controls. - calcInterpolatingControls(); + calcInterpolatingControls(m_scaledVars); // Set variable bounds. // -------------------- @@ -749,7 +749,6 @@ Solution Transcription::solve(const Iterate& guessOrig) { } auto x = flattenVariables(m_scaledVectorVars); - std::cout << "variables: " << x << std::endl; casadi_int numVariables = x.numel(); // The m_constraints symbolic vector holds all of the expressions for @@ -819,6 +818,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { Solution solution = m_problem.createIterate(); const auto finalVariables = nlpResult.at("x"); solution.variables = unscaleVariables(expandVariables(finalVariables)); + calcInterpolatingControls(solution.variables); solution.objective = nlpResult.at("f").scalar(); casadi::DMVector finalVarsDMV{finalVariables}; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 03efc07ec3..4d0f996e03 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -203,7 +203,10 @@ class Transcription { /// @note The returned vector must be a row vector of length m_numGridPoints /// with nonzero values at the mesh indices. virtual casadi::DM createMeshIndicesImpl() const = 0; - // TODO + /// Override this function to specify the indicies in the grid where the + /// control points lie. + /// @note The returned vector must be a row vector of length m_numGridPoints + /// with nonzero values at the control indices. virtual casadi::DM createControlIndicesImpl() const = 0; /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. @@ -212,7 +215,18 @@ class Transcription { casadi::MX& defects) const = 0; /// Override this function in your derived class to interpolate controls /// for time points where control variables are not defined. - virtual void calcInterpolatingControlsImpl(casadi::MX& controls) const = 0; + virtual void calcInterpolatingControlsImpl(casadi::MX& controls) const { + OPENSIM_THROW_IF(casadi::DM::all(createControlIndices()).scalar() == 0, + OpenSim::Exception, + "Must provide scheme for interpolating controls."); + } + /// Override this function in your derived class to interpolate controls + /// for time points where control variables are not defined. + virtual void calcInterpolatingControlsImpl(casadi::DM& controls) const { + OPENSIM_THROW_IF(casadi::DM::all(createControlIndices()).scalar() == 0, + OpenSim::Exception, + "Must provide scheme for interpolating controls."); + } /// Override this function to define the order of variables in the flattened /// variable vector passed to nlpsol(). Returns a vector whose elements are /// pairs of variable keys and trajectory indexes. @@ -225,9 +239,10 @@ class Transcription { m_unscaledVars.at(initial_time), m_unscaledVars.at(final_time), m_unscaledVars.at(parameters), m_constraints.defects); } - void calcInterpolatingControls() { + template + void calcInterpolatingControls(Variables& vars) { if (m_solver.getInterpolateControlMidpoints()) { - calcInterpolatingControlsImpl(m_scaledVars.at(controls)); + calcInterpolatingControlsImpl(vars.at(controls)); } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index b314b6605c..8ce4782eb0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -74,11 +74,6 @@ void Trapezoidal::calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, } } -void Trapezoidal::calcInterpolatingControlsImpl(casadi::MX& controls) const { - // For trapezoidal transcription, the control variables require no - // interpolation. -} - std::vector> Trapezoidal::getVariableOrder() const { std::vector> order; for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index 9cb1511340..fe3bc69c41 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -46,7 +46,6 @@ class Trapezoidal : public Transcription { void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; - void calcInterpolatingControlsImpl(casadi::MX& controls) const override; std::vector> getVariableOrder() const override; }; From c05a28ec6e3509bdb2bc198ab58be69d13b1e571 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 14 Aug 2024 16:54:48 -0700 Subject: [PATCH 18/38] Apply unscaleVariables and calcInterpolatingControls to callback function --- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 4 +++- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h | 4 ++-- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 9eb6373296..bc8212e209 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -60,7 +60,9 @@ class NlpsolCallback : public casadi::Callback { std::vector eval(const std::vector& args) const override { if (m_callbackInterval > 0 && evalCount % m_callbackInterval == 0) { Iterate iterate = m_problem.createIterate(); - iterate.variables = m_transcription.expandVariables(args.at(0)); + iterate.variables = m_transcription.unscaleVariables( + m_transcription.expandVariables(args.at(0))); + m_transcription.calcInterpolatingControls(iterate.variables); iterate.iteration = evalCount; iterate.times = m_transcription.createTimes(iterate.variables[initial_time], diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 4d0f996e03..f61fe26e75 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -240,7 +240,7 @@ class Transcription { m_unscaledVars.at(parameters), m_constraints.defects); } template - void calcInterpolatingControls(Variables& vars) { + void calcInterpolatingControls(Variables& vars) const { if (m_solver.getInterpolateControlMidpoints()) { calcInterpolatingControlsImpl(vars.at(controls)); } @@ -299,7 +299,7 @@ class Transcription { /// unscaled = (upper - lower) * scaled - 0.5 * (upper + lower); template - Variables unscaleVariables(const Variables& scaledVars) { + Variables unscaleVariables(const Variables& scaledVars) const { using casadi::DM; Variables out; From 5b6105da19ae0df59d0e132ce3efac083aa5f5c6 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 14 Aug 2024 17:23:22 -0700 Subject: [PATCH 19/38] Some cleanup and helpful comments --- OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp | 2 +- OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp | 1 - OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 10 ++++++++-- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h | 4 ---- 4 files changed, 9 insertions(+), 8 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index cc663688f8..8feef5680d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -215,7 +215,7 @@ casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, } else if (i == 9) { return fullPoint.at(derivatives)(Slice(), -1); } else if (i == 10) { - return fullPoint.at(parameters)(Slice(), -1); + return fullPoint.at(parameters)(Slice(), 0); } else if (i == 11) { // TODO: We should find a way to actually compute the integral // from fullPoint. Or, make the integral an optimization variable. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 94c48769e3..8d575dcd6d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -16,7 +16,6 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ #include "CasOCLegendreGauss.h" -#include using casadi::DM; using casadi::MX; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index bc8212e209..5d17fbb329 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -342,6 +342,12 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } m_unscaledVars = unscaleVariables(m_scaledVars); + // Convert parameter variables to trajectories of length m_numGridPoints so + // we can easily index over them (e.g., in evalOnTrajectory()). We have + // parameter values for each mesh point, and they are enforced to be + // time-invariant via constraints in calcDefects(). To maintain optimal + // sparsity structure, parameter values in the trajectories will be assigned + // based on the parameter variable at the corresponding mesh interval. m_times = MX(casadi::Sparsity::dense(1, m_numGridPoints)); m_parameters = MX(casadi::Sparsity::dense( m_problem.getNumParameters(), m_numGridPoints)); @@ -364,6 +370,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_unscaledVars[initial_time](-1); m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; + + // TODO this is incorrect for a non-uniform mesh. m_intervals = m_duration / m_numMeshIntervals; } @@ -1329,8 +1337,6 @@ casadi::MXVector Transcription::evalOnTrajectory( m_unscaledVars.at(states)(Slice(0, NQ + NU), timeIndices); } else if (inputs[i] == slacks) { mxIn[i + 1] = m_unscaledVars.at(inputs[i]); - // } else if (inputs[i] == controls) { - // mxIn[i + 1] = m_controls(Slice(), timeIndices); } else { mxIn[i + 1] = m_unscaledVars.at(inputs[i])(Slice(), timeIndices); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index f61fe26e75..a2d678af24 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -19,7 +19,6 @@ * -------------------------------------------------------------------------- */ #include "CasOCSolver.h" -#include namespace CasOC { @@ -388,17 +387,14 @@ class Transcription { // residual_0 x x // kinematic_0 x // path_0 x * - // interp_con_0 x x x // defect_1 x x x // residual_1 x x // kinematic_1 x // path_1 x * - // interp_con_1 x x x // defect_2 x x x // residual_2 x x // kinematic_2 x // path_2 x * - // interp_con_2 x x x // residual_3 x // kinematic_3 x // path_3 x From edb4919183b899dd54ed50cfa85559815b241b88 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 14 Aug 2024 17:30:25 -0700 Subject: [PATCH 20/38] Fix mesh interval calculation --- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 5d17fbb329..4361692c39 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -347,12 +347,18 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // parameter values for each mesh point, and they are enforced to be // time-invariant via constraints in calcDefects(). To maintain optimal // sparsity structure, parameter values in the trajectories will be assigned - // based on the parameter variable at the corresponding mesh interval. + // based on the parameter variable at the corresponding mesh interval. + const auto& mesh = m_solver.getMesh(); + m_intervals = MX(casadi::Sparsity::dense(1, m_numMeshIntervals)); m_times = MX(casadi::Sparsity::dense(1, m_numGridPoints)); m_parameters = MX(casadi::Sparsity::dense( m_problem.getNumParameters(), m_numGridPoints)); int N = m_numPointsPerMeshInterval - 1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + m_intervals(imesh) = (mesh[imesh + 1] - mesh[imesh]) * + (m_unscaledVars[final_time](imesh) - + m_unscaledVars[initial_time](imesh)); + int igrid = imesh * N; for (int i = 0; i < N; ++i) { m_parameters(Slice(), igrid + i) = @@ -370,9 +376,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_unscaledVars[initial_time](-1); m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; - - // TODO this is incorrect for a non-uniform mesh. - m_intervals = m_duration / m_numMeshIntervals; } void Transcription::transcribe() { From 4fd291b4a4857ceb5a528a08531a6c04b68e025c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 15 Aug 2024 13:14:51 -0700 Subject: [PATCH 21/38] Fixing sparsity for CasOCLegendreGaussRadau --- OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp | 6 ++---- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 2 ++ 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index b4b5fdcb03..9cc094e9e5 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -114,7 +114,7 @@ std::vector> LegendreGaussRadau::getVariableOrder() const { } if (m_solver.getInterpolateControlMidpoints()) { for (int d = 0; d < m_degree; ++d) { - order.push_back({controls, m_degree * imesh + d}); + order.push_back({controls, igrid + d + 1}); } } else { for (int i = 0; i < N; ++i) { @@ -133,9 +133,7 @@ std::vector> LegendreGaussRadau::getVariableOrder() const { order.push_back({initial_time, m_numMeshIntervals}); order.push_back({final_time, m_numMeshIntervals}); order.push_back({parameters, m_numMeshIntervals}); - if (!m_solver.getInterpolateControlMidpoints()) { - order.push_back({controls, m_numGridPoints - 1}); - } + order.push_back({controls, m_numGridPoints - 1}); order.push_back({multipliers, m_numGridPoints - 1}); order.push_back({derivatives, m_numGridPoints - 1}); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 4361692c39..beadc2d787 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -187,10 +187,12 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, auto it = std::find(controlIndicesVector.begin(), controlIndicesVector.end(), igrid); if (it != controlIndicesVector.end()) { + std::cout << "Control var: " << igrid << std::endl; m_scaledVectorVars[controls].push_back( MX::sym("controls_" + std::to_string(igrid), m_problem.getNumControls(), 1)); } else { + std::cout << "Symbolic var: " << igrid << std::endl; m_scaledVectorVars[controls].push_back( MX(m_problem.getNumControls(), 1)); } From 7b4190e3b0f3406a9f0341a66a283ed6cc90d20e Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 15 Aug 2024 17:01:29 -0700 Subject: [PATCH 22/38] Actually fix sparsity for LegendreGaussRadau --- OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 9cc094e9e5..9ddd0b0ed5 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -112,9 +112,9 @@ std::vector> LegendreGaussRadau::getVariableOrder() const { for (int i = 1; i < N; ++i) { order.push_back({states, igrid + i}); } - if (m_solver.getInterpolateControlMidpoints()) { - for (int d = 0; d < m_degree; ++d) { - order.push_back({controls, igrid + d + 1}); + if (m_solver.getInterpolateControlMidpoints() && imesh == 0) { + for (int i = 1; i < N; ++i) { + order.push_back({controls, igrid + i}); } } else { for (int i = 0; i < N; ++i) { From f9d407199e12a4292c5651d26e6eb9973a132c30 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 19 Aug 2024 09:55:58 -0700 Subject: [PATCH 23/38] Start adding support for endpoint constraints in FATROP --- .../exampleSlidingMass/exampleSlidingMass.cpp | 8 +++++-- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 2 ++ .../MocoCasADiSolver/CasOCTranscription.cpp | 23 ++++++++++++++----- .../MocoCasADiSolver/CasOCTranscription.h | 1 + 4 files changed, 26 insertions(+), 8 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 72f6df6d6c..a728fa9c36 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -99,14 +99,18 @@ int main() { // ----- problem.addGoal(); + auto* endpoint = problem.addGoal("endpoint"); + endpoint->setOutputPath("/body|linear_velocity"); + endpoint->setMode("endpoint_constraint"); + // Configure the solver. // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); solver.set_parallel(0); - solver.set_optim_solver("ipopt"); + solver.set_optim_solver("fatrop"); solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("legendre-gauss-radau-3"); + solver.set_transcription_scheme("legendre-gauss-3"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 4663bdb6cf..757a4207f1 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -40,6 +40,8 @@ enum Var { derivatives, /// Constant in time. parameters, + /// Variables for endpoint constraints. + endpoints, /// For internal use (never actually a key for Variables). multibody_states = 100 }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index beadc2d787..eafeb64d17 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -113,10 +113,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_problem.getNumKinematicConstraintEquations() * m_numMeshPoints; m_constraints.endpoint.resize( m_problem.getEndpointConstraintInfos().size()); + m_numEndpointConstraintEquations = 0; for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; - m_numConstraints += info.num_outputs; + m_numEndpointConstraintEquations += info.num_outputs; } + m_numConstraints += + m_numEndpointConstraintEquations * (m_numMeshIntervals - 1); m_constraints.path.resize(m_problem.getPathConstraintInfos().size()); m_numPathConstraintPoints = m_solver.getEnforcePathConstraintMidpoints() ? m_numGridPoints : m_numMeshPoints; @@ -176,6 +179,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVectorVars[parameters].push_back( MX::sym("parameters_" + std::to_string(imesh), m_problem.getNumParameters(), 1)); + m_scaledVectorVars[endpoints].push_back( + MX::sym("endpoints_" + std::to_string(imesh), + m_numEndpointConstraintEquations, 1)); } // Trajectory variables. for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { @@ -187,12 +193,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, auto it = std::find(controlIndicesVector.begin(), controlIndicesVector.end(), igrid); if (it != controlIndicesVector.end()) { - std::cout << "Control var: " << igrid << std::endl; m_scaledVectorVars[controls].push_back( MX::sym("controls_" + std::to_string(igrid), m_problem.getNumControls(), 1)); } else { - std::cout << "Symbolic var: " << igrid << std::endl; m_scaledVectorVars[controls].push_back( MX(m_problem.getNumControls(), 1)); } @@ -223,6 +227,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVars[initial_time] = MX::horzcat(m_scaledVectorVars[initial_time]); m_scaledVars[final_time] = MX::horzcat(m_scaledVectorVars[final_time]); m_scaledVars[parameters] = MX::horzcat(m_scaledVectorVars[parameters]); + m_scaledVars[endpoints] = MX::horzcat(m_scaledVectorVars[endpoints]); m_scaledVars[states] = MX::horzcat(m_scaledVectorVars[states]); m_scaledVars[controls] = MX::horzcat(m_scaledVectorVars[controls]); m_scaledVars[multipliers] = MX::horzcat(m_scaledVectorVars[multipliers]); @@ -678,6 +683,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_constraintsUpperBounds.endpoint.resize(numEndpointConstraints); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; + m_constraints.endpoint[iec] = MX(casadi::Sparsity::dense( + info.num_outputs, m_numMeshPoints)); + m_constraintsLowerBounds.endpoint[iec] = + DM::zeros(info.num_outputs, m_numMeshPoints); + m_constraintsUpperBounds.endpoint[iec] = + DM::zeros(info.num_outputs, m_numMeshPoints); MX integral; if (info.integrand_function) { @@ -705,9 +716,9 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_unscaledVars[parameters](Slice(), -1), integral}, endpointOut); - m_constraints.endpoint[iec] = endpointOut.at(0); - m_constraintsLowerBounds.endpoint[iec] = info.lowerBounds; - m_constraintsUpperBounds.endpoint[iec] = info.upperBounds; + m_constraints.endpoint[iec](Slice(), -1) = endpointOut.at(0); + m_constraintsLowerBounds.endpoint[iec](Slice(), -1) = info.lowerBounds; + m_constraintsUpperBounds.endpoint[iec](Slice(), -1) = info.upperBounds; } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index a2d678af24..c7a24fb952 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -159,6 +159,7 @@ class Transcription { int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; int m_numParameterConstraints = -1; + int m_numEndpointConstraintEquations = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; casadi::DM m_grid; From a9616f42f1c42d0f206a426ee4a494b518753681 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 26 Aug 2024 16:28:03 -0700 Subject: [PATCH 24/38] Resolve build errors --- OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h | 5 +++-- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 6 +++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 2848dfb5f3..f8ce2dde1e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -74,8 +74,9 @@ class HermiteSimpson : public Transcription { casadi::DM createQuadratureCoefficientsImpl() const override; casadi::DM createMeshIndicesImpl() const override; casadi::DM createControlIndicesImpl() const override; - void calcDefectsImpl(const casadi::MX& x, const casadi::MX& xdot, - const casadi::MX& ti, const casadi::MX& tf, const casadi::MX& p, + void calcDefectsImpl(const casadi::MXVector& x, + const casadi::MXVector& xdot, const casadi::MX& ti, + const casadi::MX& tf, const casadi::MX& p, casadi::MX& defects) const override; std::vector> getVariableOrder() const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index cdabd0b8af..5445c19509 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -209,7 +209,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, MX::sym("states_" + std::to_string(igrid), m_problem.getNumStates(), 1)); - if (m_solver.getInterpolateControlMidpoints()) { + if (m_solver.getInterpolateControlMeshInteriorPoints()) { auto it = std::find(controlIndicesVector.begin(), controlIndicesVector.end(), igrid); if (it != controlIndicesVector.end()) { @@ -240,8 +240,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // slack variables are applied at the mesh points at the end of each // mesh interval. for (int iproj = 0; iproj < m_numMeshIntervals-1; ++iproj) { - m_scaledVarsVars[slacks].push_back( - MX::sym("slacks_" + std::string(imesh), + m_scaledVectorVars[slacks].push_back( + MX::sym("slacks_" + std::string(iproj), m_problem.getNumSlacks(), 1)); } } else { From 5329b2d47c058677ad0aa49c9c69299b05387cf0 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 28 Aug 2024 17:05:47 -0700 Subject: [PATCH 25/38] Still working on support for endpoint constraints in FATROP --- .../exampleSlidingMass/exampleSlidingMass.cpp | 12 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 15 +- .../MocoCasADiSolver/CasOCHermiteSimpson.h | 4 +- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 4 +- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 28 ++-- .../MocoCasADiSolver/CasOCLegendreGauss.h | 4 +- .../CasOCLegendreGaussRadau.cpp | 18 +-- .../CasOCLegendreGaussRadau.h | 4 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 135 ++++++++++++++---- .../MocoCasADiSolver/CasOCTranscription.h | 65 ++++++--- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 24 +--- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 4 +- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 8 +- 13 files changed, 199 insertions(+), 126 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index a728fa9c36..df9c7f8ae1 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -99,15 +99,17 @@ int main() { // ----- problem.addGoal(); - auto* endpoint = problem.addGoal("endpoint"); - endpoint->setOutputPath("/body|linear_velocity"); - endpoint->setMode("endpoint_constraint"); + // auto* vel = problem.addGoal("velocity_integral_bound"); + // vel->setOutputPath("/body|linear_velocity"); + // vel->setMode("endpoint_constraint"); + // vel->setEndpointConstraintBounds({{-250, 250}}); // Configure the solver. // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); - solver.set_num_mesh_intervals(50); - solver.set_parallel(0); + solver.set_num_mesh_intervals(20); + solver.set_optim_finite_difference_scheme("forward"); + // solver.set_parallel(0); solver.set_optim_solver("fatrop"); solver.set_optim_hessian_approximation("exact"); solver.set_transcription_scheme("legendre-gauss-3"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 3cb2ed7c31..55f74464e6 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -57,8 +57,7 @@ DM HermiteSimpson::createControlIndicesImpl() const { } void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, const casadi::MX& tf, - const casadi::MX& p, casadi::MX& defects) const { + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); @@ -66,7 +65,7 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { // We enforce defect constraints on a mesh interval basis, so add // constraints until the number of mesh intervals is reached. - const auto h = m_times(2 * imesh + 2) - m_times(2 * imesh); + const auto h = m_intervals(imesh); const auto x_i = x[imesh](Slice(), 0); const auto x_mid = x[imesh](Slice(), 1); const auto x_ip1 = x[imesh](Slice(), 2); @@ -74,14 +73,6 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, const auto xdot_mid = xdot[imesh](Slice(), 1); const auto xdot_ip1 = xdot[imesh](Slice(), 2); - // Time variables. - defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh); - defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh); - - // Parameters. - defects(Slice(2, 2 + NP), imesh) = - p(Slice(), imesh + 1) - p(Slice(), imesh); - // Hermite interpolant defects. defects(Slice(2 + NP, 2 + NP + NS), imesh) = x_mid - 0.5 * (x_ip1 + x_i) - (h / 8.0) * (xdot_i - xdot_ip1); @@ -110,7 +101,7 @@ std::vector> HermiteSimpson::getVariableOrder() const { order.push_back({parameters, imesh}); order.push_back({states, igrid}); order.push_back({states, igrid + 1}); - if (m_solver.getInterpolateControlMidpoints()) { + if (m_solver.getInterpolateControlMeshInteriorPoints()) { order.push_back({controls, igrid}); } else { order.push_back({controls, igrid}); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index f8ce2dde1e..b870c967bb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -75,9 +75,7 @@ class HermiteSimpson : public Transcription { casadi::DM createMeshIndicesImpl() const override; casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, - const casadi::MX& tf, const casadi::MX& p, - casadi::MX& defects) const override; + const casadi::MXVector& xdot, casadi::MX& defects) const override; std::vector> getVariableOrder() const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; void calcInterpolatingControlsImpl(casadi::DM& controls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index f6249bdf49..d91b712be7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -43,8 +43,8 @@ enum Var { /// A "mirror" of the multibody states used in the projection method /// for solving kinematic constraints. projection_states, - /// Variables for endpoint constraints. - endpoints, + /// Variables for integral constraints. + integrals, /// For internal use (never actually a key for Variables). multibody_states = 100 }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index a1bce41c1c..77e0575d9b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -66,16 +66,14 @@ DM LegendreGauss::createControlIndicesImpl() const { } void LegendreGauss::calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, - const casadi::MX& tf, const casadi::MX& p, - casadi::MX& defects) const { + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); const int NP = m_problem.getNumParameters(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * (m_degree + 1); - const auto h = m_times(igrid + m_degree + 1) - m_times(igrid); + const auto h = m_intervals(imesh); const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1)); const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1)); const auto x_ip1 = x[imesh](Slice(), m_degree + 1); @@ -84,19 +82,11 @@ void LegendreGauss::calcDefectsImpl(const casadi::MXVector& x, defects(Slice(0, NS), imesh) = x_ip1 - MX::mtimes(x_i, m_interpolationCoefficients); - // Time variables. - defects(Slice(NS, NS + 1), imesh) = ti(imesh + 1) - ti(imesh); - defects(Slice(NS + 1, NS + 2), imesh) = tf(imesh + 1) - tf(imesh); - - // Parameters. - defects(Slice(NS + 2, NS + 2 + NP), imesh) = - p(Slice(), imesh + 1) - p(Slice(), imesh); - // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); for (int d = 0; d < m_degree; ++d) { - const int istart = (d + 1) * NS + 2 + NP; - const int iend = (d + 2) * NS + 2 + NP; + const int istart = (d + 1) * NS; + const int iend = (d + 2) * NS; defects(Slice(istart, iend), imesh) = residual(Slice(), d); } } @@ -115,14 +105,13 @@ std::vector> LegendreGauss::getVariableOrder() const { int N = m_numPointsPerMeshInterval - 1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - order.push_back({states, igrid}); order.push_back({initial_time, imesh}); order.push_back({final_time, imesh}); order.push_back({parameters, imesh}); - for (int i = 1; i < N; ++i) { + for (int i = 0; i < N; ++i) { order.push_back({states, igrid + i}); } - if (m_solver.getInterpolateControlMidpoints()) { + if (m_solver.getInterpolateControlMeshInteriorPoints()) { for (int d = 0; d < m_degree; ++d) { order.push_back({controls, igrid + d + 1}); } @@ -138,12 +127,15 @@ std::vector> LegendreGauss::getVariableOrder() const { order.push_back({derivatives, igrid + i}); } order.push_back({slacks, imesh}); + if (imesh < m_numMeshIntervals - 1) { + order.push_back({integrals, imesh}); + } } order.push_back({states, m_numGridPoints - 1}); order.push_back({initial_time, m_numMeshIntervals}); order.push_back({final_time, m_numMeshIntervals}); order.push_back({parameters, m_numMeshIntervals}); - if (!m_solver.getInterpolateControlMidpoints()) { + if (!m_solver.getInterpolateControlMeshInteriorPoints()) { order.push_back({controls, m_numGridPoints - 1}); } order.push_back({multipliers, m_numGridPoints - 1}); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 02d4764f99..9eaa27dc4c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -103,9 +103,7 @@ class LegendreGauss : public Transcription { casadi::DM createMeshIndicesImpl() const override; casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, - const casadi::MX& tf, const casadi::MX& p, - casadi::MX& defects) const override; + const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; void calcInterpolatingControlsImpl(casadi::DM& controls) const override; std::vector> getVariableOrder() const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 828174dbb0..ecf2558c97 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -59,26 +59,17 @@ DM LegendreGaussRadau::createControlIndicesImpl() const { } void LegendreGaussRadau::calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, const casadi::MX& tf, - const casadi::MX& p, casadi::MX& defects) const { + const casadi::MXVector& xdot, casadi::MX& defects) const { // For more information, see doxygen documentation for the class. const int NS = m_problem.getNumStates(); const int NP = m_problem.getNumParameters(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { const int igrid = imesh * m_degree; - const auto h = m_times(igrid + m_degree) - m_times(igrid); + const auto h = m_intervals(imesh); const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1)); const auto xdot_i = xdot[imesh](Slice(), Slice(1, m_degree + 1)); - // Time variables. - defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh); - defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh); - - // Parameters. - defects(Slice(2, 2 + NP), imesh) = - p(Slice(), imesh + 1) - p(Slice(), imesh); - // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); for (int d = 0; d < m_degree; ++d) { @@ -104,14 +95,13 @@ std::vector> LegendreGaussRadau::getVariableOrder() const { int N = m_numPointsPerMeshInterval - 1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - order.push_back({states, igrid}); order.push_back({initial_time, imesh}); order.push_back({final_time, imesh}); order.push_back({parameters, imesh}); - for (int i = 1; i < N; ++i) { + for (int i = 0; i < N; ++i) { order.push_back({states, igrid + i}); } - if (m_solver.getInterpolateControlMidpoints() && imesh == 0) { + if (m_solver.getInterpolateControlMeshInteriorPoints() && imesh == 0) { for (int i = 1; i < N; ++i) { order.push_back({controls, igrid + i}); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index 1a0e967e4c..a9f76d69a8 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -106,9 +106,7 @@ class LegendreGaussRadau : public Transcription { casadi::DM createMeshIndicesImpl() const override; casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, - const casadi::MX& tf, const casadi::MX& p, - casadi::MX& defects) const override; + const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; void calcInterpolatingControlsImpl(casadi::DM& controls) const override; std::vector> getVariableOrder() const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 5445c19509..2b68cf997d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -96,10 +96,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numMeshPoints = (int)m_solver.getMesh().size(); m_numGridPoints = (int)grid.numel(); m_numMeshIntervals = m_numMeshPoints - 1; - m_numParameterConstraints = m_numMeshIntervals; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; - m_numDefectsPerMeshInterval = numDefectsPerMeshInterval + - m_problem.getNumParameters() + 2; + m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; m_numPointsPerMeshInterval = numPointsPerMeshInterval; m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() @@ -110,6 +108,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numUDotErrorPoints = m_problem.isKinematicConstraintMethodBordalba2023() ? m_numGridPoints : m_numMeshPoints; + // Dynamics constraints. m_numConstraints = m_numDefectsPerMeshInterval * m_numMeshIntervals + m_numMultibodyResiduals * m_numGridPoints + @@ -118,15 +117,23 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_problem.getNumUErr() * m_numMeshPoints + m_problem.getNumUDotErr() * m_numUDotErrorPoints + m_problem.getNumProjectionConstraintEquations() * m_numMeshIntervals; + // Time constraints. + m_numConstraints += 2 * m_numMeshIntervals; + // Parameter constraints. + m_numConstraints += m_problem.getNumParameters() * m_numMeshIntervals; + // Endpoint constraints. m_constraints.endpoint.resize( m_problem.getEndpointConstraintInfos().size()); - m_numEndpointConstraintEquations = 0; + m_numIntegrals = 0; for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; - m_numEndpointConstraintEquations += info.num_outputs; + m_numConstraints += info.num_outputs; + if (info.integrand_function) { + m_numIntegrals += 1; + } } - m_numConstraints += - m_numEndpointConstraintEquations * (m_numMeshIntervals - 1); + m_numConstraints += m_numIntegrals * (m_numMeshIntervals - 1); + // Path constraints. m_constraints.path.resize(m_problem.getPathConstraintInfos().size()); m_numPathConstraintPoints = m_solver.getEnforcePathConstraintMeshInteriorPoints() @@ -199,9 +206,11 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVectorVars[parameters].push_back( MX::sym("parameters_" + std::to_string(imesh), m_problem.getNumParameters(), 1)); - m_scaledVectorVars[endpoints].push_back( - MX::sym("endpoints_" + std::to_string(imesh), - m_numEndpointConstraintEquations, 1)); + } + for (int imesh = 0; imesh < m_numMeshIntervals-1; ++imesh) { + m_scaledVectorVars[integrals].push_back( + MX::sym("integrals_" + std::to_string(imesh), + m_numIntegrals, 1)); } // Trajectory variables. for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { @@ -241,7 +250,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // mesh interval. for (int iproj = 0; iproj < m_numMeshIntervals-1; ++iproj) { m_scaledVectorVars[slacks].push_back( - MX::sym("slacks_" + std::string(iproj), + MX::sym("slacks_" + std::to_string(iproj), m_problem.getNumSlacks(), 1)); } } else { @@ -259,7 +268,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVars[initial_time] = MX::horzcat(m_scaledVectorVars[initial_time]); m_scaledVars[final_time] = MX::horzcat(m_scaledVectorVars[final_time]); m_scaledVars[parameters] = MX::horzcat(m_scaledVectorVars[parameters]); - m_scaledVars[endpoints] = MX::horzcat(m_scaledVectorVars[endpoints]); + m_scaledVars[integrals] = MX::horzcat(m_scaledVectorVars[integrals]); m_scaledVars[states] = MX::horzcat(m_scaledVectorVars[states]); m_scaledVars[controls] = MX::horzcat(m_scaledVectorVars[controls]); m_scaledVars[multipliers] = MX::horzcat(m_scaledVectorVars[multipliers]); @@ -269,6 +278,19 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Interplate controls. calcInterpolatingControls(m_scaledVars); + // Each vector contains MX matrix elements (states or state derivatives) + // needed to construct the defect constraints for an individual mesh + // interval. + m_statesByMeshInterval = MXVector(m_numMeshIntervals); + m_stateDerivativesByMeshInterval = MXVector(m_numMeshIntervals); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + m_statesByMeshInterval[imesh] = + MX(m_problem.getNumStates(), m_numPointsPerMeshInterval); + m_stateDerivativesByMeshInterval[imesh] = + MX(m_problem.getNumStates(), m_numPointsPerMeshInterval); + } + m_projectionStateDistances = MX(m_numProjectionStates, m_numMeshIntervals); + // Set variable bounds. // -------------------- auto initializeBoundsDM = [&](VariablesDM& bounds) { @@ -393,6 +415,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, ++ip; } } + // TODO how to bound integrals? + { + for (int iintg = 0; iintg < m_numIntegrals; ++iintg) { + setVariableBounds(integrals, iintg, Slice(), {-casadi::inf, casadi::inf}); + setVariableScaling(integrals, iintg, Slice(), {-casadi::inf, casadi::inf}); + } + } m_unscaledVars = unscaleVariables(m_scaledVars); // Convert parameter variables to trajectories of length m_numGridPoints so @@ -484,8 +513,21 @@ void Transcription::transcribe() { "Problems with differing numbers of coordinates and speeds are " "not supported (e.g., quaternions)."); - // TODO: Does creating all this memory have efficiency implications - // in CasADi? + // Initialize memory for time constraints. + // --------------------------------------- + m_constraints.time = MX(casadi::Sparsity::dense(2, m_numMeshIntervals)); + m_constraintsLowerBounds.time = DM::zeros(2, m_numMeshIntervals); + m_constraintsUpperBounds.time = DM::zeros(2, m_numMeshIntervals); + + // Initialize memory for parameter constraints. + // -------------------------------------------- + m_constraints.parameters = MX(casadi::Sparsity::dense( + m_problem.getNumParameters(), m_numMeshIntervals)); + m_constraintsLowerBounds.parameters = + DM::zeros(m_problem.getNumParameters(), m_numMeshIntervals); + m_constraintsUpperBounds.parameters = + DM::zeros(m_problem.getNumParameters(), m_numMeshIntervals); + // Initialize memory for state derivatives and defects. // ---------------------------------------------------- m_xdot = MX(NS, m_numGridPoints); @@ -784,6 +826,8 @@ void Transcription::transcribe() { } } + // TODO remove duplicate code since this is similar to how the states + // MXVector above casadi_int istart = 0; int numStates = m_problem.getNumStates(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { @@ -820,6 +864,18 @@ void Transcription::transcribe() { // ------------------ calcDefects(); + // Time and parameter constraints. + // ------------------------------ + const auto& ti = m_unscaledVars.at(initial_time); + const auto& tf = m_unscaledVars.at(final_time); + const auto& p = m_unscaledVars.at(parameters); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + m_constraints.time(0, imesh) = ti(imesh + 1) - ti(imesh); + m_constraints.time(1, imesh) = tf(imesh + 1) - tf(imesh); + m_constraints.parameters(Slice(), imesh) = + p(Slice(), imesh + 1) - p(Slice(), imesh); + } + // Path constraints // ---------------- // The individual path constraint functions are passed to CasADi to @@ -966,14 +1022,15 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_constraints.endpoint.resize(numEndpointConstraints); m_constraintsLowerBounds.endpoint.resize(numEndpointConstraints); m_constraintsUpperBounds.endpoint.resize(numEndpointConstraints); + m_constraints.integral = MX(casadi::Sparsity::dense( + m_numIntegrals, m_numMeshIntervals-1)); + m_constraintsLowerBounds.integral = + DM::zeros(m_numIntegrals, m_numMeshIntervals-1); + m_constraintsUpperBounds.integral = + DM::zeros(m_numIntegrals, m_numMeshIntervals-1); + int iintegral = 0; for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; - m_constraints.endpoint[iec] = MX(casadi::Sparsity::dense( - info.num_outputs, m_numMeshPoints)); - m_constraintsLowerBounds.endpoint[iec] = - DM::zeros(info.num_outputs, m_numMeshPoints); - m_constraintsUpperBounds.endpoint[iec] = - DM::zeros(info.num_outputs, m_numMeshPoints); MX integral; if (info.integrand_function) { @@ -981,7 +1038,34 @@ void Transcription::setObjectiveAndEndpointConstraints() { {states, controls, multipliers, derivatives}, m_gridIndices) .at(0); - integral = m_duration(-1) * dot(quadCoeffs.T(), integrandTraj); + std::cout << "integrandTraj: " << integrandTraj.size() << std::endl; + std::cout << "quadCoeffs: " << quadCoeffs.size() << std::endl; + std::cout << "m_duration: " << m_duration.size() << std::endl; + + int N = m_numPointsPerMeshInterval - 1; + std::cout << "m_scaledVars[integrals]: " << m_scaledVars[integrals].size() << std::endl; + for (int imesh = 0; imesh < m_numMeshIntervals-1; ++imesh) { + int igrid = imesh * N; + + const auto integ_sum_i = m_duration(imesh) * dot(quadCoeffs.T()(Slice(), Slice(igrid, igrid+N)), integrandTraj(Slice(), Slice(igrid, igrid+N))); + if (imesh > 0) { + const auto& integ_var_im1 = m_scaledVars[integrals](iintegral, imesh-1); + const auto& integ_var_i = m_scaledVars[integrals](iintegral, imesh); + m_constraints.integral(iintegral, imesh) = integ_var_im1 + integ_sum_i - integ_var_i; + } else { + const auto& integ_var_i = m_scaledVars[integrals](iintegral, imesh); + m_constraints.integral(iintegral, imesh) = integ_sum_i - integ_var_i; + } + } + + + int igrid = (m_numMeshIntervals-1) * N; + const auto integ_sum_i = m_duration(-1) * dot(quadCoeffs.T()(Slice(), Slice(igrid, igrid+m_numPointsPerMeshInterval)), integrandTraj(Slice(), Slice(igrid, igrid+m_numPointsPerMeshInterval))); + integral = m_scaledVars[integrals](iintegral, -1) + integ_sum_i; + + ++iintegral; + + // integral = m_duration(0) * dot(quadCoeffs.T(), integrandTraj); } else { integral = MX::nan(1, 1); } @@ -1001,9 +1085,9 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_unscaledVars[parameters](Slice(), -1), integral}, endpointOut); - m_constraints.endpoint[iec](Slice(), -1) = endpointOut.at(0); - m_constraintsLowerBounds.endpoint[iec](Slice(), -1) = info.lowerBounds; - m_constraintsUpperBounds.endpoint[iec](Slice(), -1) = info.upperBounds; + m_constraints.endpoint[iec] = endpointOut.at(0); + m_constraintsLowerBounds.endpoint[iec] = info.lowerBounds; + m_constraintsUpperBounds.endpoint[iec] = info.upperBounds; } } @@ -1022,6 +1106,9 @@ Solution Transcription::solve(const Iterate& guessOrig) { auto guess = guessOrig.resample(guessTimes, appendProjectionStates); guess = guess.repmatParameters(m_numMeshPoints); + // TODO formalize + guess.variables[Var::integrals] = casadi::DM::zeros(m_numIntegrals, m_numMeshIntervals-1); + // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). if (guess.variables.find(Var::slacks) != guess.variables.end()) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 4ed7b71f41..19d66e6d27 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -133,11 +133,14 @@ class Transcription { template struct Constraints { + T time; + T parameters; T defects; T multibody_residuals; T auxiliary_residuals; T kinematic; T kinematic_udoterr; + T integral; std::vector endpoint; std::vector path; T projection; @@ -161,8 +164,7 @@ class Transcription { int m_numControlPoints = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; - int m_numParameterConstraints = -1; - int m_numEndpointConstraintEquations = -1; + int m_numIntegrals = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; int m_numProjectionStates = -1; @@ -230,9 +232,7 @@ class Transcription { /// Override this function in your derived class set the defect, kinematic, /// and path constraint errors required for your transcription scheme. virtual void calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, - const casadi::MX& tf, const casadi::MX& p, - casadi::MX& defects) const = 0; + const casadi::MXVector& xdot, casadi::MX& defects) const = 0; /// Override this function in your derived class to interpolate controls /// for time points where control variables are not defined. virtual void calcInterpolatingControlsImpl(casadi::MX& controls) const { @@ -255,13 +255,12 @@ class Transcription { void transcribe(); void setObjectiveAndEndpointConstraints(); void calcDefects() { - calcDefectsImpl(m_statesByMeshInterval, m_stateDerivativesByMeshInterval, - m_unscaledVars.at(initial_time), m_unscaledVars.at(final_time), - m_unscaledVars.at(parameters), m_constraints.defects); + calcDefectsImpl(m_statesByMeshInterval, + m_stateDerivativesByMeshInterval, m_constraints.defects); } template void calcInterpolatingControls(Variables& vars) const { - if (m_solver.getInterpolateControlMidpoints()) { + if (m_solver.getInterpolateControlMeshInteriorPoints()) { calcInterpolatingControlsImpl(vars.at(controls)); } } @@ -438,9 +437,7 @@ class Transcription { // residual_3 x // 0 0.5 1 1.5 2 2.5 3 - for (const auto& endpoint : constraints.endpoint) { - copyColumn(endpoint, 0); - } + // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; @@ -448,6 +445,12 @@ class Transcription { for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; + // Time constraints. + copyColumn(constraints.time, imesh); + + // Parameter constraints. + copyColumn(constraints.parameters, imesh); + // Defect constraints. copyColumn(constraints.defects, imesh); @@ -480,6 +483,11 @@ class Transcription { // Projection constraints. copyColumn(constraints.projection, imesh); + + // Integral constraints. + if (imesh < m_numMeshIntervals - 1) { + copyColumn(constraints.integral, imesh); + } } // Final grid point. @@ -499,6 +507,10 @@ class Transcription { } } + for (const auto& endpoint : constraints.endpoint) { + copyColumn(endpoint, 0); + } + OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " "constraints should be equal to the number of constraints."); @@ -515,7 +527,9 @@ class Transcription { return T(casadi::Sparsity::dense(numRows, numColumns)); }; Constraints out; - out.defects = init(m_numDefectsPerMeshInterval, m_numMeshPoints - 1); + out.time = init(2, m_numMeshIntervals); + out.parameters = init(m_problem.getNumParameters(), m_numMeshIntervals); + out.defects = init(m_numDefectsPerMeshInterval, m_numMeshIntervals); out.multibody_residuals = init(m_numMultibodyResiduals, m_numGridPoints); out.auxiliary_residuals = init(m_numAuxiliaryResiduals, @@ -531,6 +545,7 @@ class Transcription { } out.projection = init(m_problem.getNumProjectionConstraintEquations(), m_numMeshIntervals); + out.integral = init(m_numIntegrals, m_numMeshIntervals-1); out.endpoint.resize(m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; @@ -552,16 +567,18 @@ class Transcription { } }; - for (auto& endpoint : out.endpoint) { - copyColumn(endpoint, 0); - } - // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int icon = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; + // Time constraints. + copyColumn(out.time, imesh); + + // Parameter constraints. + copyColumn(out.parameters, imesh); + // Defect constraints. copyColumn(out.defects, imesh); @@ -580,7 +597,7 @@ class Transcription { } // Path constraints. - if (m_solver.getEnforcePathConstraintMidpoints()) { + if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { for (int i = 0; i < N; ++i) { for (auto& path : out.path) { copyColumn(path, igrid + i); @@ -591,6 +608,14 @@ class Transcription { copyColumn(path, imesh); } } + + // Projection constraints. + copyColumn(out.projection, imesh); + + // Integral constraints. + if (imesh < m_numMeshIntervals - 1) { + copyColumn(out.integral, imesh); + } } // Final grid point. @@ -610,6 +635,10 @@ class Transcription { } } + for (auto& endpoint : out.endpoint) { + copyColumn(endpoint, 0); + } + OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " "constraints should be equal to the number of constraints."); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index b17cd9be46..d893a1a3b8 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -45,29 +45,19 @@ DM Trapezoidal::createControlIndicesImpl() const { } void Trapezoidal::calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, - const casadi::MX& tf, const casadi::MX& p, - casadi::MX& defects) const { + const casadi::MXVector& xdot, casadi::MX& defects) const { // We have arranged the code this way so that all constraints at a given // mesh point are grouped together (organizing the sparsity of the Jacobian // this way might have benefits for sparse linear algebra). const int NS = m_problem.getNumStates(); const int NP = m_problem.getNumParameters(); - for (int itime = 0; itime < m_numMeshIntervals; ++itime) { - const auto h = m_times(itime + 1) - m_times(itime); - const auto x_i = x[itime](Slice(), 0); - const auto x_ip1 = x[itime](Slice(), 1); - const auto xdot_i = xdot[itime](Slice(), 0); - const auto xdot_ip1 = xdot[itime](Slice(), 1); - - // Time variables. - defects(Slice(0, 1), imesh) = ti(imesh + 1) - ti(imesh); - defects(Slice(1, 2), imesh) = tf(imesh + 1) - tf(imesh); - - // Parameters. - defects(Slice(2, 2 + NP), imesh) = - p(Slice(), imesh + 1) - p(Slice(), imesh); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + const auto h = m_intervals(imesh); + const auto x_i = x[imesh](Slice(), 0); + const auto x_ip1 = x[imesh](Slice(), 1); + const auto xdot_i = xdot[imesh](Slice(), 0); + const auto xdot_ip1 = xdot[imesh](Slice(), 1); // Trapezoidal defects. defects(Slice(2 + NP, 2 + NP + NS), imesh) = diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index 3de61a2a7b..94bb58e186 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -38,9 +38,7 @@ class Trapezoidal : public Transcription { casadi::DM createMeshIndicesImpl() const override; casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MXVector& x, - const casadi::MXVector& xdot, const casadi::MX& ti, - const casadi::MX& tf, const casadi::MX& p, - casadi::MX& defects) const override; + const casadi::MXVector& xdot, casadi::MX& defects) const override; std::vector> getVariableOrder() const override; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 4f160001d0..d98c05c4c7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -117,10 +117,10 @@ inline CasOC::Iterate convertToCasOCIterate(const MocoTrajectory& mocoTraj, convertToCasADiDMTranspose(mocoTraj.getDerivativesTrajectory()); } - const SimTK::RowVector& parameters = mocoIt.getParameters(); - SimTK::Matrix parametersTrajectory(mocoIt.getNumTimes(), - mocoIt.getNumParameters()); - for (int i = 0; i < mocoIt.getNumTimes(); ++i) { + const SimTK::RowVector& parameters = mocoTraj.getParameters(); + SimTK::Matrix parametersTrajectory(mocoTraj.getNumTimes(), + mocoTraj.getNumParameters()); + for (int i = 0; i < mocoTraj.getNumTimes(); ++i) { parametersTrajectory.updRow(i) = parameters; } casVars[Var::parameters] = convertToCasADiDMTranspose(parametersTrajectory); From b9601a6b990b67366fcabe9b464e469b8e02849a Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 6 Sep 2024 15:53:50 -0700 Subject: [PATCH 26/38] Making solving kinematic constraints compatible with FATROP --- .../exampleSlidingMass/exampleSlidingMass.cpp | 18 ++- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 4 +- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 2 - .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 12 +- .../CasOCLegendreGaussRadau.cpp | 4 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 106 +++++------------- .../MocoCasADiSolver/CasOCTranscription.h | 36 +++--- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 2 +- 8 files changed, 61 insertions(+), 123 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index df9c7f8ae1..1cc660845f 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -37,6 +37,7 @@ #include #include #include +#include using namespace OpenSim; @@ -51,6 +52,8 @@ std::unique_ptr createSlidingMassModel() { auto* joint = new SliderJoint("slider", model->getGround(), *body); auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); coord.setName("position"); + coord.setPrescribedFunction(LinearFunction(1, -1)); + coord.setDefaultIsPrescribed(true); model->addComponent(joint); auto* actu = new CoordinateActuator(); @@ -86,8 +89,7 @@ int main() { // Position must be within [-5, 5] throughout the motion. // Initial position must be 0, final position must be 1. - problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), - MocoInitialBounds(0), MocoFinalBounds(1)); + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5)); // Speed must be within [-50, 50] throughout the motion. // Initial and final speed must be 0. Use compact syntax. problem.setStateInfo("/slider/position/speed", {-50, 50}, 0, 0); @@ -99,20 +101,16 @@ int main() { // ----- problem.addGoal(); - // auto* vel = problem.addGoal("velocity_integral_bound"); - // vel->setOutputPath("/body|linear_velocity"); - // vel->setMode("endpoint_constraint"); - // vel->setEndpointConstraintBounds({{-250, 250}}); - // Configure the solver. // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); - solver.set_num_mesh_intervals(20); - solver.set_optim_finite_difference_scheme("forward"); + solver.set_num_mesh_intervals(50); // solver.set_parallel(0); solver.set_optim_solver("fatrop"); + // solver.set_optim_sparsity_detection("random"); solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("legendre-gauss-3"); + solver.set_transcription_scheme("legendre-gauss-1"); + solver.set_kinematic_constraint_method("Bordalba2023"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 55f74464e6..077c867146 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -74,11 +74,11 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, const auto xdot_ip1 = xdot[imesh](Slice(), 2); // Hermite interpolant defects. - defects(Slice(2 + NP, 2 + NP + NS), imesh) = + defects(Slice(0, NS), imesh) = x_mid - 0.5 * (x_ip1 + x_i) - (h / 8.0) * (xdot_i - xdot_ip1); // Simpson integration defects. - defects(Slice(2 + NP + NS, 2 + NP + 2*NS), imesh) = + defects(Slice(NS, 2*NS), imesh) = x_ip1 - x_i - (h / 6.0) * (xdot_ip1 + 4.0 * xdot_mid + xdot_i); } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index d91b712be7..50a58b9c6c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -43,8 +43,6 @@ enum Var { /// A "mirror" of the multibody states used in the projection method /// for solving kinematic constraints. projection_states, - /// Variables for integral constraints. - integrals, /// For internal use (never actually a key for Variables). multibody_states = 100 }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 77e0575d9b..a6ca60f81f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -108,6 +108,10 @@ std::vector> LegendreGauss::getVariableOrder() const { order.push_back({initial_time, imesh}); order.push_back({final_time, imesh}); order.push_back({parameters, imesh}); + if (imesh > 0) { + order.push_back({projection_states, imesh-1}); + order.push_back({slacks, imesh-1}); + } for (int i = 0; i < N; ++i) { order.push_back({states, igrid + i}); } @@ -126,15 +130,13 @@ std::vector> LegendreGauss::getVariableOrder() const { for (int i = 0; i < N; ++i) { order.push_back({derivatives, igrid + i}); } - order.push_back({slacks, imesh}); - if (imesh < m_numMeshIntervals - 1) { - order.push_back({integrals, imesh}); - } } - order.push_back({states, m_numGridPoints - 1}); order.push_back({initial_time, m_numMeshIntervals}); order.push_back({final_time, m_numMeshIntervals}); order.push_back({parameters, m_numMeshIntervals}); + order.push_back({projection_states, m_numMeshIntervals-1}); + order.push_back({slacks, m_numMeshIntervals-1}); + order.push_back({states, m_numGridPoints - 1}); if (!m_solver.getInterpolateControlMeshInteriorPoints()) { order.push_back({controls, m_numGridPoints - 1}); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index ecf2558c97..eefb36dc94 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -73,8 +73,8 @@ void LegendreGaussRadau::calcDefectsImpl(const casadi::MXVector& x, // Residual function defects. MX residual = h * xdot_i - MX::mtimes(x_i, m_differentiationMatrix); for (int d = 0; d < m_degree; ++d) { - const int istart = d * NS + 2 + NP; - const int iend = (d + 1) * NS + 2 + NP; + const int istart = d * NS; + const int iend = (d + 1) * NS; defects(Slice(istart, iend), imesh) = residual(Slice(), d); } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 2b68cf997d..fdc751511f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -124,15 +124,10 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Endpoint constraints. m_constraints.endpoint.resize( m_problem.getEndpointConstraintInfos().size()); - m_numIntegrals = 0; for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; m_numConstraints += info.num_outputs; - if (info.integrand_function) { - m_numIntegrals += 1; - } } - m_numConstraints += m_numIntegrals * (m_numMeshIntervals - 1); // Path constraints. m_constraints.path.resize(m_problem.getPathConstraintInfos().size()); m_numPathConstraintPoints = @@ -200,22 +195,17 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Parameter variables. for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { m_scaledVectorVars[initial_time].push_back( - MX::sym("initial_time_" + std::to_string(imesh), 1, 1)); + MX::sym(fmt::format("initial_time_{}", imesh), 1, 1)); m_scaledVectorVars[final_time].push_back( - MX::sym("final_time_" + std::to_string(imesh), 1, 1)); + MX::sym(fmt::format("final_time_{}", imesh), 1, 1)); m_scaledVectorVars[parameters].push_back( - MX::sym("parameters_" + std::to_string(imesh), + MX::sym(fmt::format("parameters_{}", imesh), m_problem.getNumParameters(), 1)); } - for (int imesh = 0; imesh < m_numMeshIntervals-1; ++imesh) { - m_scaledVectorVars[integrals].push_back( - MX::sym("integrals_" + std::to_string(imesh), - m_numIntegrals, 1)); - } // Trajectory variables. for (int igrid = 0; igrid < m_numGridPoints; ++igrid) { m_scaledVectorVars[states].push_back( - MX::sym("states_" + std::to_string(igrid), + MX::sym(fmt::format("states_{}", igrid), m_problem.getNumStates(), 1)); if (m_solver.getInterpolateControlMeshInteriorPoints()) { @@ -223,7 +213,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, controlIndicesVector.end(), igrid); if (it != controlIndicesVector.end()) { m_scaledVectorVars[controls].push_back( - MX::sym("controls_" + std::to_string(igrid), + MX::sym(fmt::format("controls_{}", igrid), m_problem.getNumControls(), 1)); } else { m_scaledVectorVars[controls].push_back( @@ -231,26 +221,31 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } else { m_scaledVectorVars[controls].push_back( - MX::sym("controls_" + std::to_string(igrid), + MX::sym(fmt::format("controls_{}", igrid), m_problem.getNumControls(), 1)); } m_scaledVectorVars[multipliers].push_back( - MX::sym("multipliers_" + std::to_string(igrid), + MX::sym(fmt::format("multipliers_{}", igrid), m_problem.getNumMultipliers(), 1)); m_scaledVectorVars[derivatives].push_back( - MX::sym("derivatives_" + std::to_string(igrid), + MX::sym(fmt::format("derivatives_{}", igrid), m_problem.getNumDerivatives(), 1)); } + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + m_scaledVectorVars[projection_states].push_back(MX::sym( + fmt::format("projection_states_{}", imesh), + m_numProjectionStates, 1)); + } if (m_problem.isKinematicConstraintMethodBordalba2023()) { // In the projection method for enforcing kinematic constraints, the // slack variables are applied at the mesh points at the end of each // mesh interval. - for (int iproj = 0; iproj < m_numMeshIntervals-1; ++iproj) { + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { m_scaledVectorVars[slacks].push_back( - MX::sym("slacks_" + std::to_string(iproj), + MX::sym(fmt::format("slacks_{}", imesh), m_problem.getNumSlacks(), 1)); } } else { @@ -259,7 +254,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Hermite-Simpson collocation scheme. for (int imesh = 0; imesh < m_numMeshInteriorPoints; ++imesh) { m_scaledVectorVars[slacks].push_back( - MX::sym("slacks_" + std::to_string(imesh), + MX::sym(fmt::format("slacks_{}", imesh), m_problem.getNumSlacks(), 1)); } } @@ -268,8 +263,9 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVars[initial_time] = MX::horzcat(m_scaledVectorVars[initial_time]); m_scaledVars[final_time] = MX::horzcat(m_scaledVectorVars[final_time]); m_scaledVars[parameters] = MX::horzcat(m_scaledVectorVars[parameters]); - m_scaledVars[integrals] = MX::horzcat(m_scaledVectorVars[integrals]); m_scaledVars[states] = MX::horzcat(m_scaledVectorVars[states]); + m_scaledVars[projection_states] = + MX::horzcat(m_scaledVectorVars[projection_states]); m_scaledVars[controls] = MX::horzcat(m_scaledVectorVars[controls]); m_scaledVars[multipliers] = MX::horzcat(m_scaledVectorVars[multipliers]); m_scaledVars[derivatives] = MX::horzcat(m_scaledVectorVars[derivatives]); @@ -311,12 +307,11 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, initializeScalingDM(m_shift); initializeScalingDM(m_scale); + Bounds infBounds = {-casadi::inf, casadi::inf}; setVariableBounds(initial_time, 0, 0, m_problem.getTimeInitialBounds()); setVariableBounds(final_time, 0, 0, m_problem.getTimeFinalBounds()); - setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), - {-casadi::inf, casadi::inf}); - setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), - {-casadi::inf, casadi::inf}); + setVariableBounds(initial_time, 0, Slice(1, m_numMeshPoints), infBounds); + setVariableBounds(final_time, 0, Slice(1, m_numMeshPoints), infBounds); setVariableScaling(initial_time, 0, Slice(), m_problem.getTimeInitialBounds()); @@ -333,7 +328,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, setVariableBounds(states, is, 0, info.initialBounds); // The "-1" grabs the last column (last mesh point). setVariableBounds(states, is, -1, info.finalBounds); - setVariableScaling(states, Slice(), Slice(), info.bounds); + setVariableScaling(states, is, Slice(), info.bounds); ++is; } } @@ -346,7 +341,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, info.bounds); setVariableBounds(controls, ic, 0, info.initialBounds); setVariableBounds(controls, ic, -1, info.finalBounds); - setVariableScaling(controls, Slice(), Slice(), info.bounds); + setVariableScaling(controls, ic, Slice(), info.bounds); ++ic; } } @@ -358,7 +353,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, info.bounds); setVariableBounds(multipliers, im, 0, info.initialBounds); setVariableBounds(multipliers, im, -1, info.finalBounds); - setVariableScaling(multipliers, Slice(), Slice(), info.bounds); + setVariableScaling(multipliers, im, Slice(), info.bounds); ++im; } } @@ -402,8 +397,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, projection_states, ips, Slice(0, m_numMeshIntervals - 1), info.bounds); setVariableBounds(projection_states, ips, -1, info.finalBounds); - setVariableScaling(projection_states, Slice(), Slice(), - info.bounds); + setVariableScaling(projection_states, ips, Slice(), info.bounds); } } { @@ -415,13 +409,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, ++ip; } } - // TODO how to bound integrals? - { - for (int iintg = 0; iintg < m_numIntegrals; ++iintg) { - setVariableBounds(integrals, iintg, Slice(), {-casadi::inf, casadi::inf}); - setVariableScaling(integrals, iintg, Slice(), {-casadi::inf, casadi::inf}); - } - } m_unscaledVars = unscaleVariables(m_scaledVars); // Convert parameter variables to trajectories of length m_numGridPoints so @@ -1022,13 +1009,6 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_constraints.endpoint.resize(numEndpointConstraints); m_constraintsLowerBounds.endpoint.resize(numEndpointConstraints); m_constraintsUpperBounds.endpoint.resize(numEndpointConstraints); - m_constraints.integral = MX(casadi::Sparsity::dense( - m_numIntegrals, m_numMeshIntervals-1)); - m_constraintsLowerBounds.integral = - DM::zeros(m_numIntegrals, m_numMeshIntervals-1); - m_constraintsUpperBounds.integral = - DM::zeros(m_numIntegrals, m_numMeshIntervals-1); - int iintegral = 0; for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; @@ -1038,46 +1018,19 @@ void Transcription::setObjectiveAndEndpointConstraints() { {states, controls, multipliers, derivatives}, m_gridIndices) .at(0); - std::cout << "integrandTraj: " << integrandTraj.size() << std::endl; - std::cout << "quadCoeffs: " << quadCoeffs.size() << std::endl; - std::cout << "m_duration: " << m_duration.size() << std::endl; - - int N = m_numPointsPerMeshInterval - 1; - std::cout << "m_scaledVars[integrals]: " << m_scaledVars[integrals].size() << std::endl; - for (int imesh = 0; imesh < m_numMeshIntervals-1; ++imesh) { - int igrid = imesh * N; - - const auto integ_sum_i = m_duration(imesh) * dot(quadCoeffs.T()(Slice(), Slice(igrid, igrid+N)), integrandTraj(Slice(), Slice(igrid, igrid+N))); - if (imesh > 0) { - const auto& integ_var_im1 = m_scaledVars[integrals](iintegral, imesh-1); - const auto& integ_var_i = m_scaledVars[integrals](iintegral, imesh); - m_constraints.integral(iintegral, imesh) = integ_var_im1 + integ_sum_i - integ_var_i; - } else { - const auto& integ_var_i = m_scaledVars[integrals](iintegral, imesh); - m_constraints.integral(iintegral, imesh) = integ_sum_i - integ_var_i; - } - } - - - int igrid = (m_numMeshIntervals-1) * N; - const auto integ_sum_i = m_duration(-1) * dot(quadCoeffs.T()(Slice(), Slice(igrid, igrid+m_numPointsPerMeshInterval)), integrandTraj(Slice(), Slice(igrid, igrid+m_numPointsPerMeshInterval))); - integral = m_scaledVars[integrals](iintegral, -1) + integ_sum_i; - - ++iintegral; - - // integral = m_duration(0) * dot(quadCoeffs.T(), integrandTraj); + integral = m_duration(0) * dot(quadCoeffs.T(), integrandTraj); } else { integral = MX::nan(1, 1); } MXVector endpointOut; info.endpoint_function->call( - {m_unscaledVars[initial_time](0), + {m_unscaledVars[initial_time](-1), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), - m_unscaledVars[final_time](0), + m_unscaledVars[final_time](-1), m_unscaledVars[states](Slice(), -1), m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), @@ -1106,9 +1059,6 @@ Solution Transcription::solve(const Iterate& guessOrig) { auto guess = guessOrig.resample(guessTimes, appendProjectionStates); guess = guess.repmatParameters(m_numMeshPoints); - // TODO formalize - guess.variables[Var::integrals] = casadi::DM::zeros(m_numIntegrals, m_numMeshIntervals-1); - // Adjust guesses for the slack variables to ensure they are the correct // length (i.e. slacks.size2() == m_numPointsIgnoringConstraints). if (guess.variables.find(Var::slacks) != guess.variables.end()) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 19d66e6d27..0c3a493abd 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -80,7 +80,6 @@ class Transcription { /// overridden virtual methods are accessible to the base class. This /// implementation allows initialization to occur during construction, /// avoiding an extra call on the instantiated object. - /// TODO control points void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, int numPointsPerMeshInterval); @@ -140,7 +139,6 @@ class Transcription { T auxiliary_residuals; T kinematic; T kinematic_udoterr; - T integral; std::vector endpoint; std::vector path; T projection; @@ -164,7 +162,6 @@ class Transcription { int m_numControlPoints = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; - int m_numIntegrals = -1; int m_numConstraints = -1; int m_numPathConstraintPoints = -1; int m_numProjectionStates = -1; @@ -437,7 +434,9 @@ class Transcription { // residual_3 x // 0 0.5 1 1.5 2 2.5 3 - + for (const auto& endpoint : constraints.endpoint) { + copyColumn(endpoint, 0); + } // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; @@ -482,17 +481,15 @@ class Transcription { } // Projection constraints. - copyColumn(constraints.projection, imesh); - - // Integral constraints. - if (imesh < m_numMeshIntervals - 1) { - copyColumn(constraints.integral, imesh); + if (imesh > 0) { + copyColumn(constraints.projection, imesh-1); } } // Final grid point. copyColumn(constraints.multibody_residuals, m_numGridPoints - 1); copyColumn(constraints.auxiliary_residuals, m_numGridPoints - 1); + copyColumn(constraints.projection, m_numMeshIntervals-1); copyColumn(constraints.kinematic, m_numMeshPoints - 1); if (m_problem.isKinematicConstraintMethodBordalba2023()) { copyColumn(constraints.kinematic_udoterr, m_numGridPoints - 1); @@ -507,10 +504,6 @@ class Transcription { } } - for (const auto& endpoint : constraints.endpoint) { - copyColumn(endpoint, 0); - } - OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " "constraints should be equal to the number of constraints."); @@ -545,7 +538,6 @@ class Transcription { } out.projection = init(m_problem.getNumProjectionConstraintEquations(), m_numMeshIntervals); - out.integral = init(m_numIntegrals, m_numMeshIntervals-1); out.endpoint.resize(m_problem.getEndpointConstraintInfos().size()); for (int iec = 0; iec < (int)m_constraints.endpoint.size(); ++iec) { const auto& info = m_problem.getEndpointConstraintInfos()[iec]; @@ -566,6 +558,10 @@ class Transcription { iflat += matrix.rows(); } }; + + for (auto& endpoint : out.endpoint) { + copyColumn(endpoint, 0); + } // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; @@ -610,11 +606,8 @@ class Transcription { } // Projection constraints. - copyColumn(out.projection, imesh); - - // Integral constraints. - if (imesh < m_numMeshIntervals - 1) { - copyColumn(out.integral, imesh); + if (imesh > 0) { + copyColumn(out.projection, imesh-1); } } @@ -634,10 +627,7 @@ class Transcription { copyColumn(path, m_numMeshPoints - 1); } } - - for (auto& endpoint : out.endpoint) { - copyColumn(endpoint, 0); - } + copyColumn(out.projection, m_numMeshIntervals-1); OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index d893a1a3b8..ce17e8d31d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -60,7 +60,7 @@ void Trapezoidal::calcDefectsImpl(const casadi::MXVector& x, const auto xdot_ip1 = xdot[imesh](Slice(), 1); // Trapezoidal defects. - defects(Slice(2 + NP, 2 + NP + NS), imesh) = + defects(Slice(0, NS), imesh) = x_ip1 - (x_i + 0.5 * h * (xdot_ip1 + xdot_i)); } } From e02eb3c62780f682d5989c6ece788a8c6b03cb99 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 9 Sep 2024 13:10:52 -0700 Subject: [PATCH 27/38] Add helper to CasOCTranscription for packing states and state derivatives in MXVectors --- .../exampleSlidingMass/exampleSlidingMass.cpp | 8 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 81 +++---------------- .../MocoCasADiSolver/CasOCTranscription.h | 33 ++++++++ 3 files changed, 49 insertions(+), 73 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 1cc660845f..a824dc365c 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -52,8 +52,8 @@ std::unique_ptr createSlidingMassModel() { auto* joint = new SliderJoint("slider", model->getGround(), *body); auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); coord.setName("position"); - coord.setPrescribedFunction(LinearFunction(1, -1)); - coord.setDefaultIsPrescribed(true); + // coord.setPrescribedFunction(LinearFunction(1, -1)); + // coord.setDefaultIsPrescribed(true); model->addComponent(joint); auto* actu = new CoordinateActuator(); @@ -89,7 +89,7 @@ int main() { // Position must be within [-5, 5] throughout the motion. // Initial position must be 0, final position must be 1. - problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5)); + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), 0, 1); // Speed must be within [-50, 50] throughout the motion. // Initial and final speed must be 0. Use compact syntax. problem.setStateInfo("/slider/position/speed", {-50, 50}, 0, 0); @@ -106,7 +106,7 @@ int main() { MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); // solver.set_parallel(0); - solver.set_optim_solver("fatrop"); + solver.set_optim_solver("ipopt"); // solver.set_optim_sparsity_detection("random"); solver.set_optim_hessian_approximation("exact"); solver.set_transcription_scheme("legendre-gauss-1"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index fdc751511f..15729198bf 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -447,42 +447,8 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_duration = m_unscaledVars[final_time] - m_unscaledVars[initial_time]; // Create MXVector containing states for calcDefects(). - casadi_int istart = 0; - int numStates = m_problem.getNumStates(); - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - casadi_int numPts = m_numPointsPerMeshInterval; - casadi_int iend = istart + numPts - 1; - if (m_numProjectionStates) { - // The states at all points in the mesh interval except the last - // point are the regular state variables. - m_statesByMeshInterval[imesh](Slice(), Slice(0, numPts-1)) = - m_unscaledVars[states](Slice(), Slice(istart, iend)); - - // The multibody states at the last point in the mesh interval are - // the projection states. - m_statesByMeshInterval[imesh] - (Slice(0, m_numProjectionStates), numPts-1) = - m_unscaledVars[projection_states](Slice(), imesh); - - // The non-multibody states at the last point (i.e., auxiliary state - // variables for muscles) are also the same as the regular state - // variables (there are no projection states for these variables). - m_statesByMeshInterval[imesh]( - Slice(m_numProjectionStates, numStates), numPts-1) = - m_unscaledVars[states]( - Slice(m_numProjectionStates, numStates), iend); - - // Calculate the distance between the regular multibody states and - // the projection multibody states. - m_projectionStateDistances(Slice(), imesh) = - m_unscaledVars[projection_states](Slice(), imesh) - - m_unscaledVars[states](Slice(0, m_numProjectionStates), iend); - } else { - m_statesByMeshInterval[imesh](Slice(), Slice()) = - m_unscaledVars[states](Slice(), Slice(istart, iend+1)); - } - istart = iend; - } + convertStatesOrStateDerivativesToMXVector(m_unscaledVars[states], + m_unscaledVars[projection_states], m_statesByMeshInterval); } void Transcription::transcribe() { @@ -813,39 +779,9 @@ void Transcription::transcribe() { } } - // TODO remove duplicate code since this is similar to how the states - // MXVector above - casadi_int istart = 0; - int numStates = m_problem.getNumStates(); - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - casadi_int numPts = m_numPointsPerMeshInterval; - casadi_int iend = istart + numPts - 1; - if (m_numProjectionStates) { - // The state derivatives at all points in the mesh interval except - // the last point are the regular state derivatives. - m_stateDerivativesByMeshInterval[imesh] - (Slice(), Slice(0, numPts-1)) = - m_xdot(Slice(), Slice(istart, iend)); - - // The multibody state derivatives at the last point in the mesh - // interval are the projection state derivatives. - m_stateDerivativesByMeshInterval[imesh] - (Slice(0, m_numProjectionStates), numPts-1) = - m_xdot_projection(Slice(), imesh); - - // The non-multibody state derivatives at the last point (i.e., - // auxiliary state derivatives for muscles) are also the same as the - // regular state derivatives (there are no projection derivatives - // for these variables). - m_stateDerivativesByMeshInterval[imesh]( - Slice(m_numProjectionStates, numStates), numPts-1) = - m_xdot(Slice(m_numProjectionStates, numStates), iend); - } else { - m_stateDerivativesByMeshInterval[imesh](Slice(), Slice()) = - m_xdot(Slice(), Slice(istart, iend+1)); - } - istart = iend; - } + // Fill MXVector with state derivatives for calcDefects(). + convertStatesOrStateDerivativesToMXVector(m_xdot, m_xdot_projection, + m_stateDerivativesByMeshInterval); // Calculate defects. // ------------------ @@ -998,6 +934,13 @@ void Transcription::setObjectiveAndEndpointConstraints() { // Minimize state projection distance. if (minimizeStateProjection && m_numProjectionStates) { + for (int imesh = 0, istart = 0; imesh < m_numMeshIntervals; ++imesh) { + casadi_int iend = istart + m_numPointsPerMeshInterval - 1; + m_projectionStateDistances(Slice(), imesh) = + m_unscaledVars[projection_states](Slice(), imesh) - + m_unscaledVars[states](Slice(0, m_numProjectionStates), iend); + istart = iend; + } m_objectiveTerms(iterm++) = m_solver.getStateProjectionWeight() * MX::sum2(MX::sum1(MX::sq(m_projectionStateDistances))); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 0c3a493abd..d96c5a214f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -644,6 +644,39 @@ class Transcription { return out; } + void convertStatesOrStateDerivativesToMXVector(const casadi::MX& in, + const casadi::MX& in_proj, casadi::MXVector& out) const { + casadi_int istart = 0; + int numStates = m_problem.getNumStates(); + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + casadi_int numPts = m_numPointsPerMeshInterval; + casadi_int iend = istart + numPts - 1; + if (m_numProjectionStates) { + // The states and state derivatives at all points in the mesh + // interval except the last point are the regular state + // variables. + out[imesh](casadi::Slice(), casadi::Slice(0, numPts-1)) = + in(casadi::Slice(), casadi::Slice(istart, iend)); + + // The multibody states and state derivatives at the last point + // in the mesh interval are the projection states. + out[imesh](casadi::Slice(0, m_numProjectionStates), numPts-1) = + in_proj(casadi::Slice(), imesh); + + // The non-multibody states and state derivatives at the last + // point (i.e., auxiliary state variables for muscles) are also + // the same as the regular state variables (there are no + // projection states for these variables). + auto sliceAuxStates = + casadi::Slice(m_numProjectionStates, numStates); + out[imesh](sliceAuxStates, numPts-1) = in(sliceAuxStates, iend); + } else { + out[imesh](casadi::Slice(), casadi::Slice()) = + in(casadi::Slice(), casadi::Slice(istart, iend+1)); + } + istart = iend; + } + } friend class NlpsolCallback; }; From 9084ddde0238db8b437c5f456e21edabe2e5e5b8 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 10 Sep 2024 17:04:01 -0700 Subject: [PATCH 28/38] Add FATROP support for problems with kinematic constraints; refactor how kinematic constraints are enforced in MocoCasADiSolver --- .../exampleSlidingMass/exampleSlidingMass.cpp | 11 +- .../Moco/MocoCasADiSolver/CasOCFunction.cpp | 164 +++++---- OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h | 70 ++-- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 54 +-- .../MocoCasADiSolver/CasOCHermiteSimpson.h | 13 +- .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 115 ++++-- .../MocoCasADiSolver/CasOCLegendreGauss.h | 6 +- .../CasOCLegendreGaussRadau.cpp | 61 ++-- .../CasOCLegendreGaussRadau.h | 4 +- OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h | 68 ++-- .../MocoCasADiSolver/CasOCTranscription.cpp | 331 ++++++++---------- .../MocoCasADiSolver/CasOCTranscription.h | 148 +++++--- .../MocoCasADiSolver/CasOCTrapezoidal.cpp | 25 +- .../Moco/MocoCasADiSolver/CasOCTrapezoidal.h | 4 +- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 122 ++++--- 15 files changed, 665 insertions(+), 531 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index a824dc365c..a3822f37a1 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -52,8 +52,6 @@ std::unique_ptr createSlidingMassModel() { auto* joint = new SliderJoint("slider", model->getGround(), *body); auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); coord.setName("position"); - // coord.setPrescribedFunction(LinearFunction(1, -1)); - // coord.setDefaultIsPrescribed(true); model->addComponent(joint); auto* actu = new CoordinateActuator(); @@ -89,7 +87,8 @@ int main() { // Position must be within [-5, 5] throughout the motion. // Initial position must be 0, final position must be 1. - problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), 0, 1); + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), + MocoInitialBounds(0), MocoFinalBounds(1)); // Speed must be within [-50, 50] throughout the motion. // Initial and final speed must be 0. Use compact syntax. problem.setStateInfo("/slider/position/speed", {-50, 50}, 0, 0); @@ -105,12 +104,6 @@ int main() { // ===================== MocoCasADiSolver& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(50); - // solver.set_parallel(0); - solver.set_optim_solver("ipopt"); - // solver.set_optim_sparsity_detection("random"); - solver.set_optim_hessian_approximation("exact"); - solver.set_transcription_scheme("legendre-gauss-1"); - solver.set_kinematic_constraint_method("Bordalba2023"); // Now that we've finished setting up the tool, print it to a file. study.print("sliding_mass.omoco"); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index 279ffa3416..f4ad5dc4dc 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -241,8 +241,7 @@ VectorDM EndpointConstraint::eval(const VectorDM& args) const { return out; } -template -casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( +casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( casadi_int i) { if (i == 0) { return casadi::Sparsity::dense( @@ -254,19 +253,13 @@ casadi::Sparsity MultibodySystemExplicit::get_sparsity_out( return casadi::Sparsity::dense( m_casProblem->getNumAuxiliaryResidualEquations(), 1); } else if (i == 3) { - if (calcKCErr) { - return casadi::Sparsity::dense( - m_casProblem->getNumKinematicConstraintEquations(), 1); - } else { - return casadi::Sparsity(0, 0); - } + return casadi::Sparsity::dense(m_casProblem->getNumUDotErr(), 1); } else { return casadi::Sparsity(0, 0); } } -template -VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { +VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), args.at(3), args.at(4), args.at(5)}; VectorDM out((int)n_out()); @@ -275,19 +268,90 @@ VectorDM MultibodySystemExplicit::eval(const VectorDM& args) const { } Problem::MultibodySystemExplicitOutput output{out[0], out[1], out[2], out[3]}; - m_casProblem->calcMultibodySystemExplicit(input, calcKCErr, output); + m_casProblem->calcMultibodySystemExplicit(input, output); return out; } -template class CasOC::MultibodySystemExplicit; -template class CasOC::MultibodySystemExplicit; +casadi::Sparsity MultibodySystemImplicit::get_sparsity_out(casadi_int i) { + if (i == 0) { + return casadi::Sparsity::dense( + m_casProblem->getNumMultibodyDynamicsEquations(), 1); + } else if (i == 1) { + return casadi::Sparsity::dense( + m_casProblem->getNumAuxiliaryStates(), 1); + } else if (i == 2) { + return casadi::Sparsity::dense( + m_casProblem->getNumAuxiliaryResidualEquations(), 1); + } else if (i == 3) { + return casadi::Sparsity::dense(m_casProblem->getNumUDotErr(), 1); + } else { + return casadi::Sparsity(0, 0); + } +} -casadi::Sparsity VelocityCorrection::get_sparsity_in(casadi_int i) { +VectorDM MultibodySystemImplicit::eval(const VectorDM& args) const { + Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), + args.at(3), args.at(4), args.at(5)}; + VectorDM out((int)n_out()); + for (casadi_int i = 0; i < n_out(); ++i) { + out[i] = casadi::DM(sparsity_out(i)); + } + + Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], + out[3]}; + m_casProblem->calcMultibodySystemImplicit(input, output); + return out; +} + +casadi::Sparsity KinematicConstraints::get_sparsity_in(casadi_int i) { if (i == 0) { return casadi::Sparsity::dense(1, 1); } else if (i == 1) { + return casadi::Sparsity::dense(m_casProblem->getNumMultibodyStates(), 1); + } else if (i == 2) { + return casadi::Sparsity::dense(m_casProblem->getNumParameters(), 1); + } else { + return casadi::Sparsity(0, 0); + } +} + +casadi::Sparsity KinematicConstraints::get_sparsity_out(casadi_int i) { + if (i == 0) { return casadi::Sparsity::dense( - m_casProblem->getNumMultibodyStates(), 1); + m_casProblem->getNumQErr() + m_casProblem->getNumUErr(), 1); + } else { + return casadi::Sparsity(0, 0); + } +} + +VectorDM KinematicConstraints::eval(const VectorDM& args) const { + VectorDM out{casadi::DM(sparsity_out(0))}; + m_casProblem->calcKinematicConstraintErrors( + args.at(0).scalar(), args.at(1), args.at(2), out[0]); + return out; +} + +casadi::DM KinematicConstraints::getSubsetPoint( + const VariablesDM& fullPoint, casadi_int i) const { + int itime = 0; + using casadi::Slice; + const int NMBS = m_casProblem->getNumMultibodyStates(); + if (i == 0) { + return fullPoint.at(initial_time)(itime); + } else if (i == 1) { + return fullPoint.at(states)(Slice(0, NMBS), itime); + } else if (i == 3) { + return fullPoint.at(parameters)(Slice(), itime); + } else { + return casadi::DM(); + } +} + +casadi::Sparsity VelocityCorrection::get_sparsity_in(casadi_int i) { + if (i == 0) { + return casadi::Sparsity::dense(1, 1); + } else if (i == 1) { + return casadi::Sparsity::dense(m_casProblem->getNumMultibodyStates(), 1); } else if (i == 2) { return casadi::Sparsity::dense(m_casProblem->getNumSlacks(), 1); } else if (i == 3) { @@ -305,6 +369,13 @@ casadi::Sparsity VelocityCorrection::get_sparsity_out(casadi_int i) { } } +VectorDM VelocityCorrection::eval(const VectorDM& args) const { + VectorDM out{casadi::DM(sparsity_out(0))}; + m_casProblem->calcVelocityCorrection( + args.at(0).scalar(), args.at(1), args.at(2), args.at(3), out[0]); + return out; +} + casadi::DM VelocityCorrection::getSubsetPoint( const VariablesDM& fullPoint, casadi_int i) const { int itime = 0; @@ -323,13 +394,6 @@ casadi::DM VelocityCorrection::getSubsetPoint( } } -VectorDM VelocityCorrection::eval(const VectorDM& args) const { - VectorDM out{casadi::DM(sparsity_out(0))}; - m_casProblem->calcVelocityCorrection( - args.at(0).scalar(), args.at(1), args.at(2), args.at(3), out[0]); - return out; -} - casadi::Sparsity StateProjection::get_sparsity_in(casadi_int i) { if (i == 0) { return casadi::Sparsity::dense(1, 1); @@ -354,6 +418,13 @@ casadi::Sparsity StateProjection::get_sparsity_out(casadi_int i) { } } +VectorDM StateProjection::eval(const VectorDM& args) const { + VectorDM out{casadi::DM(sparsity_out(0))}; + m_casProblem->calcStateProjection( + args.at(0).scalar(), args.at(1), args.at(2), args.at(3), out[0]); + return out; +} + casadi::DM StateProjection::getSubsetPoint( const VariablesDM& fullPoint, casadi_int i) const { int itime = 0; @@ -371,52 +442,3 @@ casadi::DM StateProjection::getSubsetPoint( return casadi::DM(); } } - -VectorDM StateProjection::eval(const VectorDM& args) const { - VectorDM out{casadi::DM(sparsity_out(0))}; - m_casProblem->calcStateProjection( - args.at(0).scalar(), args.at(1), args.at(2), args.at(3), out[0]); - return out; -} - -template -casadi::Sparsity MultibodySystemImplicit::get_sparsity_out( - casadi_int i) { - if (i == 0) { - return casadi::Sparsity::dense( - m_casProblem->getNumMultibodyDynamicsEquations(), 1); - } else if (i == 1) { - return casadi::Sparsity::dense( - m_casProblem->getNumAuxiliaryStates(), 1); - } else if (i == 2) { - return casadi::Sparsity::dense( - m_casProblem->getNumAuxiliaryResidualEquations(), 1); - } else if (i == 3) { - if (calcKCErr) { - return casadi::Sparsity::dense( - m_casProblem->getNumKinematicConstraintEquations(), 1); - } else { - return casadi::Sparsity(0, 0); - } - } else { - return casadi::Sparsity(0, 0); - } -} - -template -VectorDM MultibodySystemImplicit::eval(const VectorDM& args) const { - Problem::ContinuousInput input{args.at(0).scalar(), args.at(1), args.at(2), - args.at(3), args.at(4), args.at(5)}; - VectorDM out((int)n_out()); - for (casadi_int i = 0; i < n_out(); ++i) { - out[i] = casadi::DM(sparsity_out(i)); - } - - Problem::MultibodySystemImplicitOutput output{out[0], out[1], out[2], - out[3]}; - m_casProblem->calcMultibodySystemImplicit(input, calcKCErr, output); - return out; -} - -template class CasOC::MultibodySystemImplicit; -template class CasOC::MultibodySystemImplicit; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h index e84e9409d3..59afe6915c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.h @@ -255,8 +255,8 @@ class EndpointConstraint : public Endpoint { }; /// This function should compute forward dynamics (explicit multibody dynamics), -/// auxiliary explicit dynamics, and the errors for the kinematic constraints. -template +/// auxiliary dynamics (explicit and implicit), and the errors for the +/// acceleration-level kinematic constraints. class MultibodySystemExplicit : public Function { public: casadi_int get_n_out() override final { return 4; } @@ -265,7 +265,7 @@ class MultibodySystemExplicit : public Function { case 0: return "multibody_derivatives"; case 1: return "auxiliary_derivatives"; case 2: return "auxiliary_residuals"; - case 3: return "kinematic_constraint_errors"; + case 3: return "kinematic_constraint_udot_errors"; default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); } } @@ -273,6 +273,53 @@ class MultibodySystemExplicit : public Function { VectorDM eval(const VectorDM& args) const override; }; + +/// This function should compute implicit multibody dynamics, auxiliary dynamics +/// (explicit and implicit), and the errors for the acceleration-level kinematic +/// constraints. +class MultibodySystemImplicit : public Function { + casadi_int get_n_out() override final { return 4; } + std::string get_name_out(casadi_int i) override final { + switch (i) { + case 0: return "multibody_residuals"; + case 1: return "auxiliary_derivatives"; + case 2: return "auxiliary_residuals"; + case 3: return "kinematic_constraint_udot_errors"; + default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); + } + } + + casadi::Sparsity get_sparsity_out(casadi_int i) override final; + VectorDM eval(const VectorDM& args) const override; +}; + +/// This function should compute the errors for the position- and velocity-level +/// kinematic constraints. +class KinematicConstraints : public Function { +public: + casadi_int get_n_in() override final { return 3; } + casadi_int get_n_out() override final { return 1; } + std::string get_name_in(casadi_int i) override final { + switch (i) { + case 0: return "time"; + case 1: return "multibody_states"; + case 2: return "parameters"; + default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); + } + } + std::string get_name_out(casadi_int i) override final { + switch (i) { + case 0: return "kinematic_constraint_errors"; + default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); + } + } + casadi::Sparsity get_sparsity_in(casadi_int i) override final; + casadi::Sparsity get_sparsity_out(casadi_int i) override final; + VectorDM eval(const VectorDM& args) const override; + casadi::DM getSubsetPoint(const VariablesDM& fullPoint, + casadi_int i) const override; +}; + /// This function should compute a velocity correction term to make feasible /// problems that enforce kinematic constraints and their derivatives. class VelocityCorrection : public Function { @@ -329,23 +376,6 @@ class StateProjection : public Function { const VariablesDM& fullPoint, casadi_int i) const override; }; -template -class MultibodySystemImplicit : public Function { - casadi_int get_n_out() override final { return 4; } - std::string get_name_out(casadi_int i) override final { - switch (i) { - case 0: return "multibody_residuals"; - case 1: return "auxiliary_derivatives"; - case 2: return "auxiliary_residuals"; - case 3: return "kinematic_constraint_errors"; - default: OPENSIM_THROW(OpenSim::Exception, "Internal error."); - } - } - - casadi::Sparsity get_sparsity_out(casadi_int i) override final; - VectorDM eval(const VectorDM& args) const override; -}; - } // namespace CasOC #endif // OPENSIM_CASOCFUNCTION_H diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index 077c867146..e18685abbc 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -91,39 +91,43 @@ void HermiteSimpson::calcInterpolatingControlsImpl(casadi::DM& controls) const { calcInterpolatingControlsHelper(controls); } -std::vector> HermiteSimpson::getVariableOrder() const { - std::vector> order; +Transcription::FlattenedVariableInfo +HermiteSimpson::getFlattenedVariableInfo() const { + FlattenedVariableInfo info; int N = m_numPointsPerMeshInterval - 1; + int nx = 0; + int nu = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - order.push_back({initial_time, imesh}); - order.push_back({final_time, imesh}); - order.push_back({parameters, imesh}); - order.push_back({states, igrid}); - order.push_back({states, igrid + 1}); + info.order.push_back({initial_time, imesh}); + info.order.push_back({final_time, imesh}); + info.order.push_back({parameters, imesh}); + info.order.push_back({states, igrid}); + info.order.push_back({states, igrid + 1}); if (m_solver.getInterpolateControlMeshInteriorPoints()) { - order.push_back({controls, igrid}); + info.order.push_back({controls, igrid}); + info.order.push_back({multipliers, igrid}); + info.order.push_back({derivatives, igrid}); } else { - order.push_back({controls, igrid}); - order.push_back({controls, igrid + 1}); + info.order.push_back({controls, igrid}); + info.order.push_back({controls, igrid + 1}); + info.order.push_back({multipliers, igrid}); + info.order.push_back({multipliers, igrid + 1}); + info.order.push_back({derivatives, igrid}); + info.order.push_back({derivatives, igrid + 1}); } - for (int i = 0; i < N; ++i) { - order.push_back({multipliers, igrid + i}); - } - for (int i = 0; i < N; ++i) { - order.push_back({derivatives, igrid + i}); - } - order.push_back({slacks, imesh}); + info.order.push_back({projection_states, imesh}); + info.order.push_back({slacks, imesh}); } - order.push_back({initial_time, m_numMeshIntervals}); - order.push_back({final_time, m_numMeshIntervals}); - order.push_back({parameters, m_numMeshIntervals}); - order.push_back({states, m_numGridPoints - 1}); - order.push_back({controls, m_numGridPoints - 1}); - order.push_back({multipliers, m_numGridPoints - 1}); - order.push_back({derivatives, m_numGridPoints - 1}); + info.order.push_back({initial_time, m_numMeshIntervals}); + info.order.push_back({final_time, m_numMeshIntervals}); + info.order.push_back({parameters, m_numMeshIntervals}); + info.order.push_back({states, m_numGridPoints - 1}); + info.order.push_back({controls, m_numGridPoints - 1}); + info.order.push_back({multipliers, m_numGridPoints - 1}); + info.order.push_back({derivatives, m_numGridPoints - 1}); - return order; + return info; } } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index b870c967bb..193413e84e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -41,8 +41,8 @@ namespace CasOC { /// errors are enforced only at the mesh points. In the kinematic constraint /// method by Bordalba et al. (2023) [2], the acceleration-level constraints are /// also enforced at the collocation points. In the kinematic constraint method -/// by Posa et al. (2016) [3], the acceleration-level constraints are only enforced -/// at the mesh points. +/// by Posa et al. (2016) [3], the acceleration-level constraints are only +/// enforced at the mesh points. /// /// References /// ---------- @@ -52,7 +52,9 @@ namespace CasOC { /// [2] Bordalba, Ricard, Tobias Schoels, Lluís Ros, Josep M. Porta, and /// Moritz Diehl. "Direct collocation methods for trajectory optimization /// in constrained robotic systems." IEEE Transactions on Robotics (2023). -/// [3] TODO Posa et al. 2016 +/// [3] Posa, M., Kuindersma, S., Tedrake, R. "Optimization and stabilization of +/// trajectories for constrained dynamical systems." IEEE International +/// Conference on Robotics and Automation (2016). class HermiteSimpson : public Transcription { public: HermiteSimpson(const Solver& solver, const Problem& problem) @@ -67,7 +69,8 @@ class HermiteSimpson : public Transcription { grid(i) = .5 * (mesh[i / 2] + mesh[i / 2 + 1]); } } - createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), 3); + createVariablesAndSetBounds(grid, 2 * m_problem.getNumStates(), + 2 * m_problem.getNumStates(), 3); } private: @@ -76,7 +79,7 @@ class HermiteSimpson : public Transcription { casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MXVector& x, const casadi::MXVector& xdot, casadi::MX& defects) const override; - std::vector> getVariableOrder() const override; + FlattenedVariableInfo getFlattenedVariableInfo() const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; void calcInterpolatingControlsImpl(casadi::DM& controls) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index a6ca60f81f..6dd15bb644 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -100,50 +100,111 @@ void LegendreGauss::calcInterpolatingControlsImpl(casadi::DM& controls) const { calcInterpolatingControlsHelper(controls); } -std::vector> LegendreGauss::getVariableOrder() const { - std::vector> order; +Transcription::FlattenedVariableInfo +LegendreGauss::getFlattenedVariableInfo() const { + FlattenedVariableInfo info; int N = m_numPointsPerMeshInterval - 1; + int nx = 0; + int nu = 0; + bool lastState = false; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - order.push_back({initial_time, imesh}); - order.push_back({final_time, imesh}); - order.push_back({parameters, imesh}); - if (imesh > 0) { - order.push_back({projection_states, imesh-1}); - order.push_back({slacks, imesh-1}); + + info.order.push_back({initial_time, imesh}); + info.order.push_back({final_time, imesh}); + nx += 2; + + info.order.push_back({parameters, imesh}); + nx += m_problem.getNumParameters(); + + if (imesh && m_numProjectionStates) { + info.order.push_back({projection_states, imesh - 1}); + nx += m_numProjectionStates; + lastState = true; + + info.order.push_back({slacks, imesh - 1}); + nu += m_problem.getNumSlacks(); } + for (int i = 0; i < N; ++i) { - order.push_back({states, igrid + i}); + info.order.push_back({states, igrid + i}); + if (lastState) { + nu += m_problem.getNumStates(); + } else { + nx += m_problem.getNumStates(); + lastState = true; + } } + if (m_solver.getInterpolateControlMeshInteriorPoints()) { for (int d = 0; d < m_degree; ++d) { - order.push_back({controls, igrid + d + 1}); + info.order.push_back({controls, igrid + d + 1}); + nu += m_problem.getNumControls(); + } + for (int d = 0; d < m_degree; ++d) { + info.order.push_back({multipliers, igrid + d + 1}); + nu += m_problem.getNumMultipliers(); + } + for (int d = 0; d < m_degree; ++d) { + info.order.push_back({derivatives, igrid + d + 1}); + nu += m_problem.getNumDerivatives(); } } else { for (int i = 0; i < N; ++i) { - order.push_back({controls, igrid + i}); + info.order.push_back({controls, igrid + i}); + nu += m_problem.getNumControls(); + } + for (int i = 0; i < N; ++i) { + info.order.push_back({multipliers, igrid + i}); + nu += m_problem.getNumMultipliers(); + } + for (int i = 0; i < N; ++i) { + info.order.push_back({derivatives, igrid + i}); + nu += m_problem.getNumDerivatives(); } } - for (int i = 0; i < N; ++i) { - order.push_back({multipliers, igrid + i}); - } - for (int i = 0; i < N; ++i) { - order.push_back({derivatives, igrid + i}); - } + + info.nx.push_back(nx); + info.nu.push_back(nu); + nx = 0; + nu = 0; + lastState = false; } - order.push_back({initial_time, m_numMeshIntervals}); - order.push_back({final_time, m_numMeshIntervals}); - order.push_back({parameters, m_numMeshIntervals}); - order.push_back({projection_states, m_numMeshIntervals-1}); - order.push_back({slacks, m_numMeshIntervals-1}); - order.push_back({states, m_numGridPoints - 1}); + info.order.push_back({initial_time, m_numMeshIntervals}); + info.order.push_back({final_time, m_numMeshIntervals}); + nx += 2; + + info.order.push_back({parameters, m_numMeshIntervals}); + nx += m_problem.getNumParameters(); + + if (m_numProjectionStates) { + info.order.push_back({projection_states, m_numMeshIntervals - 1}); + nx += m_numProjectionStates; + + info.order.push_back({slacks, m_numMeshIntervals - 1}); + nu += m_problem.getNumSlacks(); + } + + info.order.push_back({states, m_numGridPoints - 1}); + if (m_numProjectionStates) { + nu += m_problem.getNumStates(); + } else { + nx += m_problem.getNumStates(); + } + if (!m_solver.getInterpolateControlMeshInteriorPoints()) { - order.push_back({controls, m_numGridPoints - 1}); + info.order.push_back({controls, m_numGridPoints - 1}); + nu += m_problem.getNumControls(); + info.order.push_back({multipliers, m_numGridPoints - 1}); + nu += m_problem.getNumMultipliers(); + info.order.push_back({derivatives, m_numGridPoints - 1}); + nu += m_problem.getNumDerivatives(); } - order.push_back({multipliers, m_numGridPoints - 1}); - order.push_back({derivatives, m_numGridPoints - 1}); - return order; + info.nx.push_back(nx); + info.nu.push_back(nu); + + return info; } } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index 9eaa27dc4c..e7f8c4ef45 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -95,7 +95,9 @@ class LegendreGauss : public Transcription { } grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, - (m_degree + 1) * m_problem.getNumStates(), m_degree + 2); + (m_degree + 1) * m_problem.getNumStates(), + m_problem.getNumStates(), + m_degree + 2); } private: @@ -106,7 +108,7 @@ class LegendreGauss : public Transcription { const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; void calcInterpolatingControlsImpl(casadi::DM& controls) const override; - std::vector> getVariableOrder() const override; + FlattenedVariableInfo getFlattenedVariableInfo() const override; int m_degree; std::vector m_legendreRoots; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index eefb36dc94..3f2a106be0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -90,43 +90,56 @@ void LegendreGaussRadau::calcInterpolatingControlsImpl( calcInterpolatingControlsHelper(controls); } -std::vector> LegendreGaussRadau::getVariableOrder() const { - std::vector> order; +Transcription::FlattenedVariableInfo +LegendreGaussRadau::getFlattenedVariableInfo() const { + FlattenedVariableInfo info; int N = m_numPointsPerMeshInterval - 1; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; - order.push_back({initial_time, imesh}); - order.push_back({final_time, imesh}); - order.push_back({parameters, imesh}); + info.order.push_back({initial_time, imesh}); + info.order.push_back({final_time, imesh}); + info.order.push_back({parameters, imesh}); + if (imesh > 0) { + info.order.push_back({projection_states, imesh-1}); + info.order.push_back({slacks, imesh-1}); + } for (int i = 0; i < N; ++i) { - order.push_back({states, igrid + i}); + info.order.push_back({states, igrid + i}); } if (m_solver.getInterpolateControlMeshInteriorPoints() && imesh == 0) { for (int i = 1; i < N; ++i) { - order.push_back({controls, igrid + i}); + info.order.push_back({controls, igrid + i}); + } + for (int i = 1; i < N; ++i) { + info.order.push_back({multipliers, igrid + i}); + } + for (int i = 1; i < N; ++i) { + info.order.push_back({derivatives, igrid + i}); } } else { for (int i = 0; i < N; ++i) { - order.push_back({controls, igrid + i}); + info.order.push_back({controls, igrid + i}); + } + for (int i = 0; i < N; ++i) { + info.order.push_back({multipliers, igrid + i}); + } + for (int i = 0; i < N; ++i) { + info.order.push_back({derivatives, igrid + i}); } } - for (int i = 0; i < N; ++i) { - order.push_back({multipliers, igrid + i}); - } - for (int i = 0; i < N; ++i) { - order.push_back({derivatives, igrid + i}); - } - order.push_back({slacks, imesh}); + } - order.push_back({states, m_numGridPoints - 1}); - order.push_back({initial_time, m_numMeshIntervals}); - order.push_back({final_time, m_numMeshIntervals}); - order.push_back({parameters, m_numMeshIntervals}); - order.push_back({controls, m_numGridPoints - 1}); - order.push_back({multipliers, m_numGridPoints - 1}); - order.push_back({derivatives, m_numGridPoints - 1}); - - return order; + info.order.push_back({initial_time, m_numMeshIntervals}); + info.order.push_back({final_time, m_numMeshIntervals}); + info.order.push_back({parameters, m_numMeshIntervals}); + info.order.push_back({projection_states, m_numMeshIntervals-1}); + info.order.push_back({slacks, m_numMeshIntervals-1}); + info.order.push_back({states, m_numGridPoints - 1}); + info.order.push_back({controls, m_numGridPoints - 1}); + info.order.push_back({multipliers, m_numGridPoints - 1}); + info.order.push_back({derivatives, m_numGridPoints - 1}); + + return info; } } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h index a9f76d69a8..8b27736f1d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.h @@ -98,7 +98,7 @@ class LegendreGaussRadau : public Transcription { grid(numGridPoints - 1) = mesh[numMeshIntervals]; createVariablesAndSetBounds(grid, m_degree * m_problem.getNumStates(), - m_degree + 1); + m_degree * m_problem.getNumStates(), m_degree + 1); } private: @@ -109,7 +109,7 @@ class LegendreGaussRadau : public Transcription { const casadi::MXVector& xdot, casadi::MX& defects) const override; void calcInterpolatingControlsImpl(casadi::MX& controls) const override; void calcInterpolatingControlsImpl(casadi::DM& controls) const override; - std::vector> getVariableOrder() const override; + FlattenedVariableInfo getFlattenedVariableInfo() const override; int m_degree; std::vector m_legendreRoots; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h index 94fc3049fd..ec900a024e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h @@ -154,13 +154,13 @@ class Problem { casadi::DM& multibody_derivatives; casadi::DM& auxiliary_derivatives; casadi::DM& auxiliary_residuals; - casadi::DM& kinematic_constraint_errors; + casadi::DM& kinematic_constraint_udot_errors; }; struct MultibodySystemImplicitOutput { casadi::DM& multibody_residuals; casadi::DM& auxiliary_derivatives; casadi::DM& auxiliary_residuals; - casadi::DM& kinematic_constraint_errors; + casadi::DM& kinematic_constraint_udot_errors; }; protected: @@ -319,9 +319,12 @@ class Problem { /// - first derivative of velocity-level constraints /// - acceleration-level constraints virtual void calcMultibodySystemExplicit(const ContinuousInput& input, - bool calcKCErrors, MultibodySystemExplicitOutput& output) const = 0; + MultibodySystemExplicitOutput& output) const = 0; virtual void calcMultibodySystemImplicit(const ContinuousInput& input, - bool calcKCErrors, MultibodySystemImplicitOutput& output) const = 0; + MultibodySystemImplicitOutput& output) const = 0; + virtual void calcKinematicConstraintErrors(const double& time, + const casadi::DM& multibody_states, const casadi::DM& parameters, + casadi::DM& kinematic_constraint_errors) const = 0; virtual void calcVelocityCorrection(const double& time, const casadi::DM& multibody_states, const casadi::DM& slacks, const casadi::DM& parameters, @@ -460,38 +463,28 @@ class Problem { } if (m_dynamicsMode == "implicit") { - // Construct a full implicit multibody system (i.e. including - // kinematic constraints). + // Construct an implicit multibody system. mutThis->m_implicitMultibodyFunc = - OpenSim::make_unique>(); + OpenSim::make_unique(); mutThis->m_implicitMultibodyFunc->constructFunction(this, "implicit_multibody_system", finiteDiffScheme, pointsForSparsityDetection); - - // Construct an implicit multibody system ignoring kinematic - // constraints. - mutThis->m_implicitMultibodyFuncIgnoringConstraints = - OpenSim::make_unique>(); - mutThis->m_implicitMultibodyFuncIgnoringConstraints - ->constructFunction(this, - "implicit_multibody_system_ignoring_constraints", - finiteDiffScheme, pointsForSparsityDetection); } else { + // Construct an explicit multibody system. mutThis->m_multibodyFunc = - OpenSim::make_unique>(); + OpenSim::make_unique(); mutThis->m_multibodyFunc->constructFunction(this, "explicit_multibody_system", finiteDiffScheme, pointsForSparsityDetection); - - mutThis->m_multibodyFuncIgnoringConstraints = - OpenSim::make_unique>(); - mutThis->m_multibodyFuncIgnoringConstraints->constructFunction(this, - "multibody_system_ignoring_constraints", finiteDiffScheme, - pointsForSparsityDetection); } if (getEnforceConstraintDerivatives() && getNumKinematicConstraintEquations()) { + mutThis->m_kinematicConstraintsFunc = + OpenSim::make_unique(); + mutThis->m_kinematicConstraintsFunc->constructFunction(this, + "kinematic_constraints", finiteDiffScheme, + pointsForSparsityDetection); if (isKinematicConstraintMethodBordalba2023()) { mutThis->m_stateProjectionFunc = OpenSim::make_unique(); @@ -642,17 +635,15 @@ class Problem { const std::vector& getPathConstraintInfos() const { return m_pathInfos; } - /// Get a function to the full multibody system (i.e. including kinematic - /// constraints errors). + /// Get a function to the full explicit multibody system (i.e., including + /// acceleration-level kinematic constraint errors). const casadi::Function& getMultibodySystem() const { return *m_multibodyFunc; } - /// Get a function to the multibody system that does *not* compute kinematic - /// constraint errors (if they exist). This may be necessary for computing - /// state derivatives at grid points where we do not want to enforce - /// kinematic constraint errors. - const casadi::Function& getMultibodySystemIgnoringConstraints() const { - return *m_multibodyFuncIgnoringConstraints; + /// Get a function to compute the position- and velocity-level kinematic + /// constraint errors. + const casadi::Function& getKinematicConstraints() const { + return *m_kinematicConstraintsFunc; } /// Get a function to compute the velocity correction to qdot when enforcing /// kinematic constraints and their derivatives using the method by @@ -667,13 +658,11 @@ class Problem { const casadi::Function& getStateProjection() const { return *m_stateProjectionFunc; } + /// Get a function to the full implicit multibody system (i.e., including + /// acceleration-level kinematic constraint errors). const casadi::Function& getImplicitMultibodySystem() const { return *m_implicitMultibodyFunc; } - const casadi::Function& - getImplicitMultibodySystemIgnoringConstraints() const { - return *m_implicitMultibodyFuncIgnoringConstraints; - } /// @} private: @@ -709,12 +698,9 @@ class Problem { std::vector m_costInfos; std::vector m_endpointConstraintInfos; std::vector m_pathInfos; - std::unique_ptr> m_multibodyFunc; - std::unique_ptr> - m_multibodyFuncIgnoringConstraints; - std::unique_ptr> m_implicitMultibodyFunc; - std::unique_ptr> - m_implicitMultibodyFuncIgnoringConstraints; + std::unique_ptr m_multibodyFunc; + std::unique_ptr m_implicitMultibodyFunc; + std::unique_ptr m_kinematicConstraintsFunc; std::unique_ptr m_velocityCorrectionFunc; std::unique_ptr m_stateProjectionFunc; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 15729198bf..72824c729e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -85,6 +85,7 @@ class NlpsolCallback : public casadi::Callback { void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numGapClosingDefectsPerMeshInterval, int numPointsPerMeshInterval) { // Set the grid. @@ -97,26 +98,40 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numGridPoints = (int)grid.numel(); m_numMeshIntervals = m_numMeshPoints - 1; m_numMeshInteriorPoints = m_numGridPoints - m_numMeshPoints; - m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; + m_numDefectsPerMeshInterval = numDefectsPerMeshInterval; + m_numGapClosingDefectsPerMeshInterval = numGapClosingDefectsPerMeshInterval; m_numPointsPerMeshInterval = numPointsPerMeshInterval; m_numMultibodyResiduals = m_problem.isDynamicsModeImplicit() ? m_problem.getNumMultibodyDynamicsEquations() : 0; m_numAuxiliaryResiduals = m_problem.getNumAuxiliaryResidualEquations(); - m_numProjectionStates = m_problem.getNumProjectionConstraintEquations(); + m_numProjectionStates = m_problem.getNumProjectionStates(); + + // Create the vector of points where control variables are defined + // (including Lagrange multipliers and derivative variables). These points + // also influence where path constraints associated with the model dynamics + // (e.g., multibody residuals) are enforced. + auto controlIndicesMap = createControlIndices(); + std::vector controlIndicesVector; + for (int i = 0; i < controlIndicesMap.size2(); ++i) { + if (controlIndicesMap(i).scalar() == 1) { + controlIndicesVector.push_back(i); + } + } + m_numControlPoints = static_cast(controlIndicesVector.size()); + m_numDynamicsPoints = m_solver.getInterpolateControlMeshInteriorPoints() ? + m_numControlPoints : m_numGridPoints; - m_numUDotErrorPoints = m_problem.isKinematicConstraintMethodBordalba2023() - ? m_numGridPoints - : m_numMeshPoints; // Dynamics constraints. m_numConstraints = m_numDefectsPerMeshInterval * m_numMeshIntervals + - m_numMultibodyResiduals * m_numGridPoints + - m_numAuxiliaryResiduals * m_numGridPoints + + m_numMultibodyResiduals * m_numDynamicsPoints + + m_numAuxiliaryResiduals * m_numDynamicsPoints + m_problem.getNumQErr() * m_numMeshPoints + m_problem.getNumUErr() * m_numMeshPoints + - m_problem.getNumUDotErr() * m_numUDotErrorPoints + + m_problem.getNumUDotErr() * m_numDynamicsPoints + m_problem.getNumProjectionConstraintEquations() * m_numMeshIntervals; + // Time constraints. m_numConstraints += 2 * m_numMeshIntervals; // Parameter constraints. @@ -160,15 +175,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } - auto controlIndicesMap = createControlIndices(); - std::vector controlIndicesVector; - for (int i = 0; i < controlIndicesMap.size2(); ++i) { - if (controlIndicesMap(i).scalar() == 1) { - controlIndicesVector.push_back(i); - } - } - m_numControlPoints = static_cast(controlIndicesVector.size()); - auto makeTimeIndices = [](const std::vector& in) { casadi::Matrix out(1, in.size()); for (int i = 0; i < (int)in.size(); ++i) { out(i) = in[i]; } @@ -186,10 +192,24 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, ? makeTimeIndices(gridIndicesVector) : makeTimeIndices(meshIndicesVector); m_controlIndices = makeTimeIndices(controlIndicesVector); + m_dynamicsIndices = m_solver.getInterpolateControlMeshInteriorPoints() ? + m_controlIndices : m_gridIndices; m_projectionStateIndices = makeTimeIndices(projectionStateIndicesVector); m_notProjectionStateIndices = makeTimeIndices(notProjectionStateIndicesVector); + std::vector projectionStateIndicesForControlIndicesVector; + for (int i = 0; i < static_cast(controlIndicesVector.size()); ++i) { + int controlIndex = controlIndicesVector[i]; + if (std::find(projectionStateIndicesVector.begin(), + projectionStateIndicesVector.end(), controlIndex) != + projectionStateIndicesVector.end()) { + projectionStateIndicesForControlIndicesVector.push_back(i); + } + } + m_projectionStateIndicesForControlIndices = + makeTimeIndices(projectionStateIndicesForControlIndicesVector); + // Create variables. // ----------------- // Parameter variables. @@ -215,22 +235,32 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_scaledVectorVars[controls].push_back( MX::sym(fmt::format("controls_{}", igrid), m_problem.getNumControls(), 1)); + m_scaledVectorVars[multipliers].push_back( + MX::sym(fmt::format("multipliers_{}", igrid), + m_problem.getNumMultipliers(), 1)); + m_scaledVectorVars[derivatives].push_back( + MX::sym(fmt::format("derivatives_{}", igrid), + m_problem.getNumDerivatives(), 1)); + } else { m_scaledVectorVars[controls].push_back( MX(m_problem.getNumControls(), 1)); + m_scaledVectorVars[multipliers].push_back( + MX(m_problem.getNumMultipliers(), 1)); + m_scaledVectorVars[derivatives].push_back( + MX(m_problem.getNumDerivatives(), 1)); } } else { m_scaledVectorVars[controls].push_back( MX::sym(fmt::format("controls_{}", igrid), m_problem.getNumControls(), 1)); - } - - m_scaledVectorVars[multipliers].push_back( + m_scaledVectorVars[multipliers].push_back( MX::sym(fmt::format("multipliers_{}", igrid), m_problem.getNumMultipliers(), 1)); - m_scaledVectorVars[derivatives].push_back( + m_scaledVectorVars[derivatives].push_back( MX::sym(fmt::format("derivatives_{}", igrid), m_problem.getNumDerivatives(), 1)); + } } for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { m_scaledVectorVars[projection_states].push_back(MX::sym( @@ -238,7 +268,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numProjectionStates, 1)); } - if (m_problem.isKinematicConstraintMethodBordalba2023()) { // In the projection method for enforcing kinematic constraints, the // slack variables are applied at the mesh points at the end of each @@ -495,20 +524,20 @@ void Transcription::transcribe() { // Initialize memory for implicit multibody residuals. // --------------------------------------------------- m_constraints.multibody_residuals = MX(casadi::Sparsity::dense( - m_numMultibodyResiduals, m_numGridPoints)); + m_numMultibodyResiduals, m_numDynamicsPoints)); m_constraintsLowerBounds.multibody_residuals = - DM::zeros(m_numMultibodyResiduals, m_numGridPoints); + DM::zeros(m_numMultibodyResiduals, m_numDynamicsPoints); m_constraintsUpperBounds.multibody_residuals = - DM::zeros(m_numMultibodyResiduals, m_numGridPoints); + DM::zeros(m_numMultibodyResiduals, m_numDynamicsPoints); // Initialize memory for implicit auxiliary residuals. // --------------------------------------------------- m_constraints.auxiliary_residuals = MX(casadi::Sparsity::dense( - m_numAuxiliaryResiduals, m_numGridPoints)); + m_numAuxiliaryResiduals, m_numDynamicsPoints)); m_constraintsLowerBounds.auxiliary_residuals = - DM::zeros(m_numAuxiliaryResiduals, m_numGridPoints); + DM::zeros(m_numAuxiliaryResiduals, m_numDynamicsPoints); m_constraintsUpperBounds.auxiliary_residuals = - DM::zeros(m_numAuxiliaryResiduals, m_numGridPoints); + DM::zeros(m_numAuxiliaryResiduals, m_numDynamicsPoints); // Initialize memory for kinematic constraints. // -------------------------------------------- @@ -518,36 +547,26 @@ void Transcription::transcribe() { int nkc = m_problem.getNumKinematicConstraintEquations(); const auto& kcBounds = m_problem.getKinematicConstraintBounds(); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - m_constraints.kinematic = MX(casadi::Sparsity::dense( - nqerr + nuerr, m_numMeshPoints)); - m_constraintsLowerBounds.kinematic = casadi::DM::repmat( - kcBounds.lower, nqerr + nuerr, m_numMeshPoints); - m_constraintsUpperBounds.kinematic = casadi::DM::repmat( - kcBounds.upper, nqerr + nuerr, m_numMeshPoints); - - m_constraints.kinematic_udoterr = MX(casadi::Sparsity::dense( - nudoterr, m_numUDotErrorPoints)); - m_constraintsLowerBounds.kinematic_udoterr = casadi::DM::repmat( - kcBounds.lower, nudoterr, m_numUDotErrorPoints); - m_constraintsUpperBounds.kinematic_udoterr = casadi::DM::repmat( - kcBounds.upper, nudoterr, m_numUDotErrorPoints); - } else { - m_constraints.kinematic = MX(casadi::Sparsity::dense( - nkc, m_numMeshPoints)); - m_constraintsLowerBounds.kinematic = casadi::DM::repmat( - kcBounds.lower, nkc, m_numMeshPoints); - m_constraintsUpperBounds.kinematic = casadi::DM::repmat( - kcBounds.upper, nkc, m_numMeshPoints); - } + m_constraints.kinematic = MX(casadi::Sparsity::dense( + nqerr + nuerr, m_numMeshPoints)); + m_constraintsLowerBounds.kinematic = casadi::DM::repmat( + kcBounds.lower, nqerr + nuerr, m_numMeshPoints); + m_constraintsUpperBounds.kinematic = casadi::DM::repmat( + kcBounds.upper, nqerr + nuerr, m_numMeshPoints); + + m_constraints.kinematic_udoterr = MX(casadi::Sparsity::dense( + nudoterr, m_numDynamicsPoints)); + m_constraintsLowerBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.lower, nudoterr, m_numDynamicsPoints); + m_constraintsUpperBounds.kinematic_udoterr = casadi::DM::repmat( + kcBounds.upper, nudoterr, m_numDynamicsPoints); // Initialize memory for projection constraints. // --------------------------------------------- int numProjectionConstraints = m_problem.getNumProjectionConstraintEquations(); - m_constraints.projection = MX( - casadi::Sparsity::dense(numProjectionConstraints, - m_numMeshIntervals)); + m_constraints.projection = MX(casadi::Sparsity::dense( + numProjectionConstraints, m_numMeshIntervals)); m_constraintsLowerBounds.projection = DM::zeros(numProjectionConstraints, m_numMeshIntervals); m_constraintsUpperBounds.projection = @@ -563,8 +582,8 @@ void Transcription::transcribe() { m_xdot_projection(Slice(0, NQ), Slice()) = u_projection; } - // projection constraints - // ---------------------- + // kinematic constraints + // --------------------- if (m_problem.getNumKinematicConstraintEquations() && !m_problem.isPrescribedKinematics() && m_problem.getEnforceConstraintDerivatives()) { @@ -577,6 +596,14 @@ void Transcription::transcribe() { "but the '{}' scheme was selected.", m_solver.getTranscriptionScheme()); + // qerr, uerr + // ---------- + const auto out = evalOnTrajectory(m_problem.getKinematicConstraints(), + {multibody_states}, m_meshIndices); + m_constraints.kinematic = out.at(0); + + // projection constraints + // ---------------------- if (m_solver.getTranscriptionScheme() == "hermite-simpson" && !m_problem.isKinematicConstraintMethodBordalba2023()) { // In Hermite-Simpson, we must compute a velocity correction at all @@ -608,8 +635,8 @@ void Transcription::transcribe() { } } - // udot, zdot, residual, kcerr - // --------------------------- + // udot, zdot, residual, udoterr + // ----------------------------- if (m_problem.isDynamicsModeImplicit()) { // udot. const MX w = m_unscaledVars[derivatives] @@ -617,70 +644,29 @@ void Transcription::transcribe() { m_xdot(Slice(NQ, NQ + NU), Slice()) = w; if (m_numProjectionStates) { const MX w_projection = m_unscaledVars[derivatives] - (Slice(0, m_problem.getNumSpeeds()), - m_projectionStateIndices); + (Slice(0, m_problem.getNumSpeeds()), m_projectionStateIndices); m_xdot_projection(Slice(NQ, NQ + NU), Slice()) = w_projection; } + // Evaluate the implicit multibody system function and get multibody + // residuals, auxiliary residuals, zdot (auxiliary derivatives), and + // udoterr (acceleration-level kinematic constraint errors). std::vector inputs{states, controls, multipliers, derivatives}; - - // When the model has kinematic constraints, we must treat grid points - // differently, as kinematic constraints are computed for only some - // grid points. When the model does *not* have kinematic constraints, - // the DAE is the same for all grid points, but the evaluation is still - // done separately to keep implementation general. - - // residual, zdot, kcerr - // Points where we compute algebraic constraints. - { - const auto out = - evalOnTrajectory(m_problem.getImplicitMultibodySystem(), - inputs, m_meshIndices); - m_constraints.multibody_residuals(Slice(), m_meshIndices) = - out.at(0); - // zdot. - m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); - m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = - out.at(2); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( - Slice(0, nqerr), Slice()); - m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) - = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); - m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = - out.at(3)( - Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), - Slice()); - } else { - m_constraints.kinematic = out.at(3); - } - } - - // Points where we ignore algebraic constraints. - if (m_numMeshInteriorPoints) { - const casadi::Function& implicitMultibodyFunction = - m_problem.isKinematicConstraintMethodBordalba2023() ? - m_problem.getImplicitMultibodySystem() : - m_problem.getImplicitMultibodySystemIgnoringConstraints(); - const auto out = evalOnTrajectory(implicitMultibodyFunction, inputs, - m_meshInteriorIndices); - m_constraints.multibody_residuals(Slice(), m_meshInteriorIndices) = - out.at(0); - // zdot. - m_xdot(Slice(NQ + NU, NS), m_meshInteriorIndices) = - out.at(1); - m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = - out.at(2); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) - = out.at(3)( - Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), - Slice()); - } - } - - // Points where the multibody residuals depend on the projection states. - if (m_numProjectionStates) { + const auto out = evalOnTrajectory(m_problem.getImplicitMultibodySystem(), + inputs, m_gridIndices); + m_constraints.multibody_residuals = + out.at(0)(Slice(), m_dynamicsIndices); + // zdot. + m_xdot(Slice(NQ + NU, NS), Slice()) = out.at(1); + m_constraints.auxiliary_residuals = + out.at(2)(Slice(), m_dynamicsIndices); + m_constraints.kinematic_udoterr = + out.at(3)(Slice(), m_dynamicsIndices); + + // Points where the multibody residuals and udoterro depend on the + // projection states. + if (m_numProjectionStates && + !m_projectionStateIndicesForControlIndices.is_empty()) { const casadi::Function& implicitMultibodyFunction = m_problem.getImplicitMultibodySystem(); const auto out = evalOnTrajectory(implicitMultibodyFunction, @@ -688,74 +674,38 @@ void Transcription::transcribe() { m_projectionStateIndices); // This overrides the previous function evaluation assignments for // `kinematic_udoterr` and `multibody_residuals` at the mesh indices - // (i.e., for "points where we compute algebraic constraints"). We - // still need the function evaluations above since we need state + // We still need the function evaluations above since we need state // derivatives for auxiliary states on the mesh points (note that // the projection states overlap with all the mesh indices except // the first mesh point). This also keeps the implementation above // general when we do not have projection states. - m_constraints.multibody_residuals( - Slice(), m_projectionStateIndices) = out.at(0); - m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) - = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), - Slice()); + m_constraints.multibody_residuals(Slice(), + m_projectionStateIndicesForControlIndices) = out.at(0); + m_constraints.kinematic_udoterr(Slice(), + m_projectionStateIndicesForControlIndices) = out.at(3); } } else { // Explicit dynamics mode. - std::vector inputs{states, controls, multipliers, derivatives}; - // udot, zdot, kcerr. - // Points where we compute algebraic constraints. - { - // Evaluate the multibody system function and get udot - // (speed derivatives) and zdot (auxiliary derivatives). - const auto out = evalOnTrajectory( - m_problem.getMultibodySystem(), inputs, m_meshIndices); - m_xdot(Slice(NQ, NQ + NU), m_meshIndices) = out.at(0); - m_xdot(Slice(NQ + NU, NS), m_meshIndices) = out.at(1); - m_constraints.auxiliary_residuals(Slice(), m_meshIndices) = - out.at(2); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - m_constraints.kinematic(Slice(0, nqerr), Slice()) = out.at(3)( - Slice(0, nqerr), Slice()); - m_constraints.kinematic(Slice(nqerr, nqerr + nuerr), Slice()) - = out.at(3)(Slice(nqerr, nqerr + nuerr), Slice()); - m_constraints.kinematic_udoterr(Slice(), m_meshIndices) = - out.at(3)( - Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), - Slice()); - } else { - m_constraints.kinematic = out.at(3); - } - } - - // Points where we ignore algebraic constraints. - if (m_numMeshInteriorPoints) { - // In the Bordalba et al. 2023 method, we need to enforce the - // acceleration-level kinematic constraints at the mesh interval - // interior points. - const casadi::Function& multibodyFunction = - m_problem.isKinematicConstraintMethodBordalba2023() ? - m_problem.getMultibodySystem() : - m_problem.getMultibodySystemIgnoringConstraints(); - const auto out = evalOnTrajectory(multibodyFunction, inputs, - m_meshInteriorIndices); - m_xdot(Slice(NQ, NQ + NU), m_meshInteriorIndices) = - out.at(0); - m_xdot(Slice(NQ + NU, NS), m_meshInteriorIndices) = - out.at(1); - m_constraints.auxiliary_residuals(Slice(), m_meshInteriorIndices) = - out.at(2); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - m_constraints.kinematic_udoterr(Slice(), m_meshInteriorIndices) - = out.at(3)( - Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), - Slice()); - } - } + // udot, zdot, udoterr + // ------------------- + std::vector inputs{states, controls, multipliers, derivatives}; - // Points where the state derivatives depend on the projection states. - if (m_numProjectionStates) { + // Evaluate the explicit multibody system function and get udot + // (speed derivatives), zdot (auxiliary derivatives), and udoterr + // (acceleration-level kinematic constraint errors). + const auto out = evalOnTrajectory(m_problem.getMultibodySystem(), + inputs, m_gridIndices); + m_xdot(Slice(NQ, NQ + NU), Slice()) = out.at(0); + m_xdot(Slice(NQ + NU, NS), Slice()) = out.at(1); + m_constraints.auxiliary_residuals = + out.at(2)(Slice(), m_dynamicsIndices); + m_constraints.kinematic_udoterr = out.at(3)(Slice(), m_dynamicsIndices); + + // Points where the state derivatives and udoterr depend on the + // projection states. + if (m_numProjectionStates && + !m_projectionStateIndicesForControlIndices.is_empty()) { const auto out = evalOnTrajectory(m_problem.getMultibodySystem(), {projection_states, controls, multipliers, derivatives}, m_projectionStateIndices); @@ -773,9 +723,8 @@ void Transcription::transcribe() { // overlap with all the mesh indices except the first mesh point). // This also keeps the implementation above general when we do not // have projection states. - m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) - = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr), - Slice()); + m_constraints.kinematic_udoterr(Slice(), + m_projectionStateIndicesForControlIndices) = out.at(3); } } @@ -1088,8 +1037,8 @@ Solution Transcription::solve(const Iterate& guessOrig) { // The m_constraints symbolic vector holds all of the expressions for // the constraint functions. - auto g = flattenConstraints(m_constraints); - casadi_int numConstraints = g.numel(); + auto flat = flattenConstraints(m_constraints); + casadi_int numConstraints = flat.constraints.numel(); NlpsolCallback callback(*this, m_problem, numVariables, numConstraints, m_solver.getCallbackInterval()); @@ -1105,7 +1054,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { objective = 0; } nlp.emplace(std::make_pair("f", objective)); - nlp.emplace(std::make_pair("g", g)); + nlp.emplace(std::make_pair("g", flat.constraints)); if (!m_solver.getWriteSparsity().empty()) { const auto prefix = m_solver.getWriteSparsity(); auto gradient = casadi::MX::gradient(nlp["f"], nlp["x"]); @@ -1124,10 +1073,20 @@ Solution Transcription::solve(const Iterate& guessOrig) { prefix + "constraint_Jacobian_sparsity.mtx"); } - const auto lbg = flattenConstraints(m_constraintsLowerBounds); - const auto ubg = flattenConstraints(m_constraintsUpperBounds); + const auto lbg = flattenConstraints(m_constraintsLowerBounds).constraints; + const auto ubg = flattenConstraints(m_constraintsUpperBounds).constraints; if (m_solver.getOptimSolver() == "fatrop") { - options["structure_detection"] = "auto"; + FlattenedVariableInfo info = getFlattenedVariableInfo(); + if (info.nx.empty() || info.nu.empty() || flat.ng.empty()) { + options["structure_detection"] = "auto"; + } else { + options["structure_detection"] = "manual"; + options["N"] = m_numMeshIntervals; + options["nx"] = info.nx; + options["nu"] = info.nu; + options["ng"] = flat.ng; + } + std::vector equality; for (int i = 0; i < m_numConstraints; ++i) { equality.push_back(lbg(i).scalar() == ubg(i).scalar()); @@ -1173,7 +1132,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { // For some reason, nlpResult.at("g") is all 0. So we calculate the // constraints ourselves. - casadi::Function constraintFunc("constraints", {x}, {g}); + casadi::Function constraintFunc("constraints", {x}, {flat.constraints}); casadi::DMVector constraintsOut; constraintFunc.call(finalVarsDMV, constraintsOut); printConstraintValues(solution, expandConstraints(constraintsOut[0])); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index d96c5a214f..2b1ec72293 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -82,6 +82,7 @@ class Transcription { /// avoiding an extra call on the instantiated object. void createVariablesAndSetBounds(const casadi::DM& grid, int numDefectsPerMeshInterval, + int numGapClosingDefectsPerMeshInterval, int numPointsPerMeshInterval); /// We assume all functions depend on time and parameters. @@ -130,6 +131,12 @@ class Transcription { } } + struct FlattenedVariableInfo { + std::vector> order; + std::vector nx; + std::vector nu; + }; + template struct Constraints { T time; @@ -143,6 +150,13 @@ class Transcription { std::vector path; T projection; }; + + template + struct FlattenedConstraints { + T constraints; + std::vector ng; + }; + void printConstraintValues(const Iterate& it, const Constraints& constraints, std::ostream& stream = std::cout) const; @@ -157,9 +171,10 @@ class Transcription { int m_numMeshIntervals = -1; int m_numMeshInteriorPoints = -1; int m_numDefectsPerMeshInterval = -1; + int m_numGapClosingDefectsPerMeshInterval = -1; int m_numPointsPerMeshInterval = -1; - int m_numUDotErrorPoints = -1; int m_numControlPoints = -1; + int m_numDynamicsPoints = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; int m_numConstraints = -1; @@ -191,8 +206,10 @@ class Transcription { casadi::Matrix m_meshInteriorIndices; casadi::Matrix m_pathConstraintIndices; casadi::Matrix m_controlIndices; + casadi::Matrix m_dynamicsIndices; casadi::Matrix m_projectionStateIndices; casadi::Matrix m_notProjectionStateIndices; + casadi::Matrix m_projectionStateIndicesForControlIndices; // State derivatives. casadi::MX m_xdot; @@ -246,8 +263,10 @@ class Transcription { } /// Override this function to define the order of variables in the flattened /// variable vector passed to nlpsol(). Returns a vector whose elements are - /// pairs of variable keys and trajectory indexes. - virtual std::vector> getVariableOrder() const = 0; + /// pairs of variable keys and trajectory indexes. Optionally, define the + /// number of states and controls for each mesh interval, which is needed by + /// the FATROP solver. + virtual FlattenedVariableInfo getFlattenedVariableInfo() const = 0; void transcribe(); void setObjectiveAndEndpointConstraints(); @@ -259,6 +278,8 @@ class Transcription { void calcInterpolatingControls(Variables& vars) const { if (m_solver.getInterpolateControlMeshInteriorPoints()) { calcInterpolatingControlsImpl(vars.at(controls)); + calcInterpolatingControlsImpl(vars.at(multipliers)); + calcInterpolatingControlsImpl(vars.at(derivatives)); } } @@ -266,8 +287,8 @@ class Transcription { /// nlpsol(), etc. casadi::MX flattenVariables(const VectorVariablesMX& vars) const { std::vector stdvec; - auto varOrder = getVariableOrder(); - for (const auto& kv : varOrder) { + auto info = getFlattenedVariableInfo(); + for (const auto& kv : info.order) { stdvec.push_back(vars.at(kv.first)[kv.second]); } @@ -278,8 +299,8 @@ class Transcription { /// nlpsol(), etc. casadi::DM flattenVariables(const VariablesDM& vars) const { std::vector stdvec; - auto varOrder = getVariableOrder(); - for (const auto& kv : varOrder) { + auto info = getFlattenedVariableInfo(); + for (const auto& kv : info.order) { if (m_scaledVars.at(kv.first).rows()) { stdvec.push_back(vars.at(kv.first)(casadi::Slice(), kv.second)); } @@ -298,8 +319,8 @@ class Transcription { kv.second.columns()); } - auto varOrder = getVariableOrder(); - for (const auto& kv : varOrder) { + auto info = getFlattenedVariableInfo(); + for (const auto& kv : info.order) { const auto& var = kv.first; const auto& index = kv.second; casadi_int size = m_scaledVars.at(var).rows(); @@ -357,14 +378,16 @@ class Transcription { /// grouped together by time. Organizing the sparsity of the Jacobian /// this way might have benefits for sparse linear algebra. template - T flattenConstraints(const Constraints& constraints) const { - T flat = T(casadi::Sparsity::dense(m_numConstraints, 1)); + FlattenedConstraints flattenConstraints( + const Constraints& constraints) const { + FlattenedConstraints flat; + flat.constraints = T(casadi::Sparsity::dense(m_numConstraints, 1)); int iflat = 0; auto copyColumn = [&flat, &iflat](const T& matrix, int columnIndex) { using casadi::Slice; if (matrix.rows()) { - flat(Slice(iflat, iflat + matrix.rows())) = + flat.constraints(Slice(iflat, iflat + matrix.rows())) = matrix(Slice(), columnIndex); iflat += matrix.rows(); } @@ -440,7 +463,11 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; - int icon = 0; + int idyn = 0; + auto dynamicsIndices = + m_solver.getInterpolateControlMeshInteriorPoints() ? + createControlIndices() : casadi::DM::ones(1, m_numGridPoints); + int ng = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; @@ -452,18 +479,24 @@ class Transcription { // Defect constraints. copyColumn(constraints.defects, imesh); + ng += m_numDefectsPerMeshInterval - + m_numGapClosingDefectsPerMeshInterval; - // Multibody and auxiliary residuals. - for (int i = 0; i < N; ++i) { - copyColumn(constraints.multibody_residuals, igrid + i); - copyColumn(constraints.auxiliary_residuals, igrid + i); - } - - // Kinematic constraints. + // Kinematic constraints (qerr and uerr). copyColumn(constraints.kinematic, imesh); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - for (int i = 0; i < N; ++i) { - copyColumn(constraints.kinematic_udoterr, igrid + i); + ng += constraints.kinematic.rows(); + + // Multibody residuals, auxiliary residuals, and acceleration-level + // kinematic constraints (udoterr). + for (int i = 0; i < N; ++i) { + if (dynamicsIndices(igrid + i).scalar() == 1) { + copyColumn(constraints.kinematic_udoterr, idyn); + ng += constraints.kinematic_udoterr.rows(); + copyColumn(constraints.multibody_residuals, idyn); + ng += constraints.multibody_residuals.rows(); + copyColumn(constraints.auxiliary_residuals, idyn); + ng += constraints.auxiliary_residuals.rows(); + ++idyn; } } @@ -472,37 +505,51 @@ class Transcription { for (int i = 0; i < N; ++i) { for (const auto& path : constraints.path) { copyColumn(path, igrid + i); + ng += path.rows(); } } } else { for (const auto& path : constraints.path) { copyColumn(path, imesh); + ng += path.rows(); } } // Projection constraints. if (imesh > 0) { - copyColumn(constraints.projection, imesh-1); + copyColumn(constraints.projection, imesh - 1); + ng += constraints.projection.rows(); } + + flat.ng.push_back(ng); + ng = 0; } // Final grid point. - copyColumn(constraints.multibody_residuals, m_numGridPoints - 1); - copyColumn(constraints.auxiliary_residuals, m_numGridPoints - 1); - copyColumn(constraints.projection, m_numMeshIntervals-1); copyColumn(constraints.kinematic, m_numMeshPoints - 1); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - copyColumn(constraints.kinematic_udoterr, m_numGridPoints - 1); + ng += constraints.kinematic.rows(); + if (dynamicsIndices(m_numGridPoints - 1).scalar() == 1) { + copyColumn(constraints.kinematic_udoterr, idyn); + ng += constraints.kinematic_udoterr.rows(); + copyColumn(constraints.multibody_residuals, idyn); + ng += constraints.multibody_residuals.rows(); + copyColumn(constraints.auxiliary_residuals, idyn); + ng += constraints.auxiliary_residuals.rows(); } if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { for (const auto& path : constraints.path) { copyColumn(path, m_numGridPoints - 1); + ng += path.rows(); } } else { for (const auto& path : constraints.path) { copyColumn(path, m_numMeshPoints - 1); + ng += path.rows(); } } + copyColumn(constraints.projection, m_numMeshIntervals - 1); + ng += constraints.projection.rows(); + flat.ng.push_back(ng); OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " @@ -532,10 +579,8 @@ class Transcription { int numUDotErr = m_problem.getNumUDotErr(); int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? numQErr + numUErr : numQErr + numUErr + numUDotErr; - out.kinematic = init(numKC,m_numMeshPoints); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - out.kinematic_udoterr = init(numUDotErr, m_numUDotErrorPoints); - } + out.kinematic = init(numQErr + numUErr, m_numMeshPoints); + out.kinematic_udoterr = init(numUDotErr, m_numDynamicsPoints); out.projection = init(m_problem.getNumProjectionConstraintEquations(), m_numMeshIntervals); out.endpoint.resize(m_problem.getEndpointConstraintInfos().size()); @@ -565,7 +610,10 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; - int icon = 0; + int idyn = 0; + auto dynamicsIndices = + m_solver.getInterpolateControlMeshInteriorPoints() ? + createControlIndices() : casadi::DM::ones(1, m_numGridPoints); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; @@ -578,17 +626,17 @@ class Transcription { // Defect constraints. copyColumn(out.defects, imesh); - // Multibody and auxiliary residuals. - for (int i = 0; i < N; ++i) { - copyColumn(out.multibody_residuals, igrid + i); - copyColumn(out.auxiliary_residuals, igrid + i); - } - - // Kinematic constraints. + // Kinematic constraints (qerr and uerr). copyColumn(out.kinematic, imesh); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - for (int i = 0; i < N; ++i) { + + // Multibody residuals, auxiliary residuals, and acceleration-level + // kinematic constraints (udoterr). + for (int i = 0; i < N; ++i) { + if (dynamicsIndices(igrid + i).scalar() == 1) { copyColumn(out.kinematic_udoterr, igrid + i); + copyColumn(out.multibody_residuals, igrid + i); + copyColumn(out.auxiliary_residuals, igrid + i); + ++idyn; } } @@ -607,16 +655,16 @@ class Transcription { // Projection constraints. if (imesh > 0) { - copyColumn(out.projection, imesh-1); + copyColumn(out.projection, imesh - 1); } } - + // Final grid point. - copyColumn(out.multibody_residuals, m_numGridPoints - 1); - copyColumn(out.auxiliary_residuals, m_numGridPoints - 1); copyColumn(out.kinematic, m_numMeshPoints - 1); - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - copyColumn(out.kinematic_udoterr, m_numGridPoints - 1); + if (dynamicsIndices(m_numGridPoints - 1).scalar() == 1) { + copyColumn(out.kinematic_udoterr, idyn); + copyColumn(out.multibody_residuals, idyn); + copyColumn(out.auxiliary_residuals, idyn); } if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { for (auto& path : out.path) { @@ -627,7 +675,7 @@ class Transcription { copyColumn(path, m_numMeshPoints - 1); } } - copyColumn(out.projection, m_numMeshIntervals-1); + copyColumn(out.projection, m_numMeshIntervals - 1); OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, "Internal error: final value of the index into the flattened " diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp index ce17e8d31d..80348936bb 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.cpp @@ -65,19 +65,24 @@ void Trapezoidal::calcDefectsImpl(const casadi::MXVector& x, } } -std::vector> Trapezoidal::getVariableOrder() const { - std::vector> order; +Transcription::FlattenedVariableInfo +Trapezoidal::getFlattenedVariableInfo() const { + FlattenedVariableInfo info; for (int imesh = 0; imesh < m_numMeshPoints; ++imesh) { - order.push_back({initial_time, imesh}); - order.push_back({final_time, imesh}); - order.push_back({parameters, imesh}); - order.push_back({states, imesh}); - order.push_back({controls, imesh}); - order.push_back({multipliers, imesh}); - order.push_back({derivatives, imesh}); + info.order.push_back({initial_time, imesh}); + info.order.push_back({final_time, imesh}); + info.order.push_back({parameters, imesh}); + info.order.push_back({states, imesh}); + info.order.push_back({controls, imesh}); + info.order.push_back({multipliers, imesh}); + info.order.push_back({derivatives, imesh}); + if (imesh) { + info.order.push_back({projection_states, imesh - 1}); + info.order.push_back({slacks, imesh - 1}); + } } - return order; + return info; } } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h index 94bb58e186..860d7b8639 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTrapezoidal.h @@ -30,7 +30,7 @@ class Trapezoidal : public Transcription { Trapezoidal(const Solver& solver, const Problem& problem) : Transcription(solver, problem) { createVariablesAndSetBounds(m_solver.getMesh(), - m_problem.getNumStates(), 2); + m_problem.getNumStates(), m_problem.getNumStates(), 2); } private: @@ -39,7 +39,7 @@ class Trapezoidal : public Transcription { casadi::DM createControlIndicesImpl() const override; void calcDefectsImpl(const casadi::MXVector& x, const casadi::MXVector& xdot, casadi::MX& defects) const override; - std::vector> getVariableOrder() const override; + FlattenedVariableInfo getFlattenedVariableInfo() const override; }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index d98c05c4c7..1ec334464e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -324,7 +324,6 @@ class MocoCasOCProblem : public CasOC::Problem { private: void calcMultibodySystemExplicit(const ContinuousInput& input, - bool calcKCErrors, MultibodySystemExplicitOutput& output) const override { auto mocoProblemRep = m_jar->take(); @@ -345,11 +344,23 @@ class MocoCasOCProblem : public CasOC::Problem { simtkStateDisabledConstraints); // Compute kinematic constraint errors. - if (getNumMultipliers() && calcKCErrors) { - calcKinematicConstraintErrors( - modelBase, simtkStateBase, - simtkStateDisabledConstraints, - output.kinematic_constraint_errors); + if (getNumMultipliers()) { + // Calculate udoterr. We cannot use State::getUDotErr() + // because that uses Simbody's multipliers and UDot, + // whereas we have our own multipliers and UDot. Here, we use + // the udot computed from the model with disabled constraints + // since we cannot use (nor do we have available) udot computed + // from the original model. + if (getEnforceConstraintDerivatives() || + getNumAccelerationConstraintEquations()) { + const auto& matter = modelBase.getMatterSubsystem(); + matter.calcConstraintAccelerationErrors(simtkStateBase, + simtkStateDisabledConstraints.getUDot(), m_pvaerr); + } else { + m_pvaerr = SimTK::NaN; + } + std::copy_n(m_pvaerr.getContiguousScalarData() + m_udoterrOffset, + m_udoterrSize, output.kinematic_constraint_udot_errors.ptr()); } // Copy state derivative values to output. @@ -367,7 +378,6 @@ class MocoCasOCProblem : public CasOC::Problem { m_jar->leave(std::move(mocoProblemRep)); } void calcMultibodySystemImplicit(const ContinuousInput& input, - bool calcKCErrors, MultibodySystemImplicitOutput& output) const override { auto mocoProblemRep = m_jar->take(); @@ -391,11 +401,23 @@ class MocoCasOCProblem : public CasOC::Problem { simtkStateDisabledConstraints); // Compute kinematic constraint errors. - if (getNumMultipliers() && calcKCErrors) { - calcKinematicConstraintErrors( - modelBase, simtkStateBase, - simtkStateDisabledConstraints, - output.kinematic_constraint_errors); + if (getNumMultipliers()) { + // Calculate udoterr. We cannot use State::getUDotErr() + // because that uses Simbody's multipliers and UDot, + // whereas we have our own multipliers and UDot. Here, we use + // the udot computed from the model with disabled constraints + // since we cannot use (nor do we have available) udot computed + // from the original model. + if (getEnforceConstraintDerivatives() || + getNumAccelerationConstraintEquations()) { + const auto& matter = modelBase.getMatterSubsystem(); + matter.calcConstraintAccelerationErrors(simtkStateBase, + simtkStateDisabledConstraints.getUDot(), m_pvaerr); + } else { + m_pvaerr = SimTK::NaN; + } + std::copy_n(m_pvaerr.getContiguousScalarData() + m_udoterrOffset, + m_udoterrSize, output.kinematic_constraint_udot_errors.ptr()); } const SimTK::SimbodyMatterSubsystem& matterDisabledConstraints = @@ -416,6 +438,37 @@ class MocoCasOCProblem : public CasOC::Problem { m_jar->leave(std::move(mocoProblemRep)); } + void calcKinematicConstraintErrors(const double& time, + const casadi::DM& multibody_states, const casadi::DM& parameters, + casadi::DM& kinematic_constraint_errors) const override { + + if (isPrescribedKinematics()) return; + auto mocoProblemRep = m_jar->take(); + + const auto& modelBase = mocoProblemRep->getModelBase(); + auto& simtkStateBase = mocoProblemRep->updStateBase(); + + // Update the model and state. + applyParametersToModelProperties(parameters, *mocoProblemRep); + convertStatesToSimTKState( + SimTK::Stage::Velocity, time, multibody_states, + modelBase, simtkStateBase, false); + modelBase.realizeVelocity(simtkStateBase); + + // Position-level errors. + const auto& qerr = simtkStateBase.getQErr(); + // Velocity-level errors. + const auto& uerr = simtkStateBase.getUErr(); + + // This way of copying the data avoids a threadsafety issue in + // CasADi related to cached Sparsity objects. + std::copy_n(qerr.getContiguousScalarData(), qerr.size(), + kinematic_constraint_errors.ptr()); + std::copy_n(uerr.getContiguousScalarData() + m_uerrOffset, m_uerrSize, + kinematic_constraint_errors.ptr() + qerr.size()); + + m_jar->leave(std::move(mocoProblemRep)); + } void calcVelocityCorrection(const double& time, const casadi::DM& multibody_states, const casadi::DM& slacks, const casadi::DM& parameters, @@ -822,51 +875,6 @@ class MocoCasOCProblem : public CasOC::Problem { m_constraintMobilityForces, m_constraintBodyForces); } - void calcKinematicConstraintErrors( - const Model& modelBase, - const SimTK::State& stateBase, - const SimTK::State& simtkStateDisabledConstraints, - casadi::DM& kinematic_constraint_errors) const { - - // If all kinematics are prescribed, we assume that the prescribed - // kinematics obey any kinematic constraints. Therefore, the kinematic - // constraints would be redundant, and we need not enforce them. - if (isPrescribedKinematics()) return; - - // Calculate udoterr. We cannot use State::getUDotErr() - // because that uses Simbody's multipliers and UDot, - // whereas we have our own multipliers and UDot. Here, we use - // the udot computed from the model with disabled constraints - // since we cannot use (nor do we have available) udot computed - // from the original model. - if (getEnforceConstraintDerivatives() || - getNumAccelerationConstraintEquations()) { - const auto& matter = modelBase.getMatterSubsystem(); - matter.calcConstraintAccelerationErrors(stateBase, - simtkStateDisabledConstraints.getUDot(), m_pvaerr); - } else { - m_pvaerr = SimTK::NaN; - } - - // Position-level errors. - const auto& qerr = stateBase.getQErr(); - // Velocity-level errors. - const auto& uerr = stateBase.getUErr(); - // Acceleration-level errors. - const auto& udoterr = m_pvaerr; - - // This way of copying the data avoids a threadsafety issue in - // CasADi related to cached Sparsity objects. - std::copy_n(qerr.getContiguousScalarData(), qerr.size(), - kinematic_constraint_errors.ptr()); - std::copy_n(uerr.getContiguousScalarData() + m_uerrOffset, m_uerrSize, - kinematic_constraint_errors.ptr() + qerr.size()); - std::copy_n(udoterr.getContiguousScalarData() + m_udoterrOffset, - m_udoterrSize, - kinematic_constraint_errors.ptr() + qerr.size() + m_uerrSize); - - } - void copyImplicitResidualsToOutput(const MocoProblemRep& mocoProblemRep, const SimTK::State& state, casadi::DM& auxiliary_residuals) const { if (getNumAuxiliaryResidualEquations()) { From 8f74b2ae2d5386b331a43842e44709c2b49fb65a Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 10 Sep 2024 17:06:51 -0700 Subject: [PATCH 29/38] Revert exampleSlidingMass --- .../Moco/exampleSlidingMass/exampleSlidingMass.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index a3822f37a1..9fc55903a6 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -37,7 +37,6 @@ #include #include #include -#include using namespace OpenSim; @@ -88,7 +87,7 @@ int main() { // Position must be within [-5, 5] throughout the motion. // Initial position must be 0, final position must be 1. problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), - MocoInitialBounds(0), MocoFinalBounds(1)); + MocoInitialBounds(0), MocoFinalBounds(1)); // Speed must be within [-50, 50] throughout the motion. // Initial and final speed must be 0. Use compact syntax. problem.setStateInfo("/slider/position/speed", {-50, 50}, 0, 0); @@ -112,11 +111,11 @@ int main() { // ================== MocoSolution solution = study.solve(); - solution.write("sliding_mass_solution.sto"); + //solution.write("sliding_mass_solution.sto"); // Visualize. // ========== - // study.visualize(solution); + study.visualize(solution); return EXIT_SUCCESS; } From 337d2ff0ffdb1f389242c922b799084b03e5b396 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 11 Sep 2024 15:40:03 -0700 Subject: [PATCH 30/38] Remove control interpolation and midpoint path constraint options in MocoCasADiSolver; lots of fixes to tests --- .../exampleSlidingMass/exampleSlidingMass.cpp | 2 +- .../MocoCasADiSolver/CasOCHermiteSimpson.cpp | 30 +- .../MocoCasADiSolver/CasOCHermiteSimpson.h | 27 +- OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h | 1 - .../MocoCasADiSolver/CasOCLegendreGauss.cpp | 46 +- .../MocoCasADiSolver/CasOCLegendreGauss.h | 2 +- .../CasOCLegendreGaussRadau.cpp | 2 +- .../Moco/MocoCasADiSolver/CasOCProblem.cpp | 3 +- OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h | 34 +- OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h | 34 +- .../MocoCasADiSolver/CasOCTranscription.cpp | 226 ++++---- .../MocoCasADiSolver/CasOCTranscription.h | 164 +++--- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 4 - OpenSim/Moco/MocoDirectCollocationSolver.h | 2 + OpenSim/Moco/Test/testMocoActuators.cpp | 506 +++++++++--------- OpenSim/Moco/Test/testMocoConstraints.cpp | 202 +++---- OpenSim/Moco/Test/testMocoInverse.cpp | 6 +- 17 files changed, 604 insertions(+), 687 deletions(-) diff --git a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp index 9fc55903a6..17eb22f15c 100644 --- a/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp +++ b/OpenSim/Examples/Moco/exampleSlidingMass/exampleSlidingMass.cpp @@ -86,7 +86,7 @@ int main() { // Position must be within [-5, 5] throughout the motion. // Initial position must be 0, final position must be 1. - problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), + problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), MocoInitialBounds(0), MocoFinalBounds(1)); // Speed must be within [-50, 50] throughout the motion. // Initial and final speed must be 0. Use compact syntax. diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp index e18685abbc..187b6ce86e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.cpp @@ -51,9 +51,7 @@ DM HermiteSimpson::createMeshIndicesImpl() const { } DM HermiteSimpson::createControlIndicesImpl() const { - DM indices = DM::zeros(1, m_numGridPoints); - for (int i = 0; i < m_numGridPoints; i += 2) { indices(i) = 1; } - return indices; + return DM::ones(1, m_numGridPoints); } void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, @@ -83,14 +81,6 @@ void HermiteSimpson::calcDefectsImpl(const casadi::MXVector& x, } } -void HermiteSimpson::calcInterpolatingControlsImpl(casadi::MX& controls) const { - calcInterpolatingControlsHelper(controls); -} - -void HermiteSimpson::calcInterpolatingControlsImpl(casadi::DM& controls) const { - calcInterpolatingControlsHelper(controls); -} - Transcription::FlattenedVariableInfo HermiteSimpson::getFlattenedVariableInfo() const { FlattenedVariableInfo info; @@ -104,18 +94,12 @@ HermiteSimpson::getFlattenedVariableInfo() const { info.order.push_back({parameters, imesh}); info.order.push_back({states, igrid}); info.order.push_back({states, igrid + 1}); - if (m_solver.getInterpolateControlMeshInteriorPoints()) { - info.order.push_back({controls, igrid}); - info.order.push_back({multipliers, igrid}); - info.order.push_back({derivatives, igrid}); - } else { - info.order.push_back({controls, igrid}); - info.order.push_back({controls, igrid + 1}); - info.order.push_back({multipliers, igrid}); - info.order.push_back({multipliers, igrid + 1}); - info.order.push_back({derivatives, igrid}); - info.order.push_back({derivatives, igrid + 1}); - } + info.order.push_back({controls, igrid}); + info.order.push_back({controls, igrid + 1}); + info.order.push_back({multipliers, igrid}); + info.order.push_back({multipliers, igrid + 1}); + info.order.push_back({derivatives, igrid}); + info.order.push_back({derivatives, igrid + 1}); info.order.push_back({projection_states, imesh}); info.order.push_back({slacks, imesh}); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h index 193413e84e..732c08ce19 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCHermiteSimpson.h @@ -39,20 +39,17 @@ namespace CasOC { /// ------------------------------------------- /// Position- and velocity-level kinematic constraint errors and path constraint /// errors are enforced only at the mesh points. In the kinematic constraint -/// method by Bordalba et al. (2023) [2], the acceleration-level constraints are +/// method by Bordalba et al. (2023) [1], the acceleration-level constraints are /// also enforced at the collocation points. In the kinematic constraint method -/// by Posa et al. (2016) [3], the acceleration-level constraints are only +/// by Posa et al. (2016) [2], the acceleration-level constraints are only /// enforced at the mesh points. /// /// References /// ---------- -/// [1] Hargraves, C.R., Paris, S.W. "Direct Trajectory Optimization Using -/// Nonlinear Programming and Collocation." Journal of Guidance, Control, -/// and Dynamics (1987). -/// [2] Bordalba, Ricard, Tobias Schoels, Lluís Ros, Josep M. Porta, and +/// [1] Bordalba, Ricard, Tobias Schoels, Lluís Ros, Josep M. Porta, and /// Moritz Diehl. "Direct collocation methods for trajectory optimization /// in constrained robotic systems." IEEE Transactions on Robotics (2023). -/// [3] Posa, M., Kuindersma, S., Tedrake, R. "Optimization and stabilization of +/// [2] Posa, M., Kuindersma, S., Tedrake, R. "Optimization and stabilization of /// trajectories for constrained dynamical systems." IEEE International /// Conference on Robotics and Automation (2016). class HermiteSimpson : public Transcription { @@ -80,22 +77,6 @@ class HermiteSimpson : public Transcription { void calcDefectsImpl(const casadi::MXVector& x, const casadi::MXVector& xdot, casadi::MX& defects) const override; FlattenedVariableInfo getFlattenedVariableInfo() const override; - void calcInterpolatingControlsImpl(casadi::MX& controls) const override; - void calcInterpolatingControlsImpl(casadi::DM& controls) const override; - - template - void calcInterpolatingControlsHelper(T& controls) const { - using casadi::Slice; - - // This control approximation scheme is based on the control scheme - // proposed by Hargraves and Paris (1987) [1]. Linear interpolation of - // controls is also recommended by Bordalba et al. (2023) [2]. - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - controls(Slice(), 2*imesh + 1) = 0.5 * ( - controls(Slice(), 2*imesh) + - controls(Slice(), 2*imesh + 2)); - } - } }; } // namespace CasOC diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h index 50a58b9c6c..699267bb50 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCIterate.h @@ -76,7 +76,6 @@ struct Iterate { bool appendProjectionStates) const; /// Make repeat copies of parameter variables (including initial and final /// time) using the same value for each time point. - /// TODO include projection variables Iterate repmatParameters(int numPoints); }; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp index 6dd15bb644..cc0b4e2c8e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp @@ -136,32 +136,17 @@ LegendreGauss::getFlattenedVariableInfo() const { } } - if (m_solver.getInterpolateControlMeshInteriorPoints()) { - for (int d = 0; d < m_degree; ++d) { - info.order.push_back({controls, igrid + d + 1}); - nu += m_problem.getNumControls(); - } - for (int d = 0; d < m_degree; ++d) { - info.order.push_back({multipliers, igrid + d + 1}); - nu += m_problem.getNumMultipliers(); - } - for (int d = 0; d < m_degree; ++d) { - info.order.push_back({derivatives, igrid + d + 1}); - nu += m_problem.getNumDerivatives(); - } - } else { - for (int i = 0; i < N; ++i) { - info.order.push_back({controls, igrid + i}); - nu += m_problem.getNumControls(); - } - for (int i = 0; i < N; ++i) { - info.order.push_back({multipliers, igrid + i}); - nu += m_problem.getNumMultipliers(); - } - for (int i = 0; i < N; ++i) { - info.order.push_back({derivatives, igrid + i}); - nu += m_problem.getNumDerivatives(); - } + for (int d = 0; d < m_degree; ++d) { + info.order.push_back({controls, igrid + d + 1}); + nu += m_problem.getNumControls(); + } + for (int d = 0; d < m_degree; ++d) { + info.order.push_back({multipliers, igrid + d + 1}); + nu += m_problem.getNumMultipliers(); + } + for (int d = 0; d < m_degree; ++d) { + info.order.push_back({derivatives, igrid + d + 1}); + nu += m_problem.getNumDerivatives(); } info.nx.push_back(nx); @@ -192,15 +177,6 @@ LegendreGauss::getFlattenedVariableInfo() const { nx += m_problem.getNumStates(); } - if (!m_solver.getInterpolateControlMeshInteriorPoints()) { - info.order.push_back({controls, m_numGridPoints - 1}); - nu += m_problem.getNumControls(); - info.order.push_back({multipliers, m_numGridPoints - 1}); - nu += m_problem.getNumMultipliers(); - info.order.push_back({derivatives, m_numGridPoints - 1}); - nu += m_problem.getNumDerivatives(); - } - info.nx.push_back(nx); info.nu.push_back(nu); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h index e7f8c4ef45..084840ae41 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h @@ -153,7 +153,7 @@ class LegendreGauss : public Transcription { controls(Slice(), m_numGridPoints - 1) = 0; for (int d = 0; d < m_degree; ++d) { const int igrid = (m_numMeshIntervals - 1) * (m_degree + 1); - const auto c_t = controls(Slice(), igrid); + const auto c_t = controls(Slice(), igrid + d + 1); controls(Slice(), m_numGridPoints - 1) += getLagrangePolynomial(d, 1) * c_t; } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp index 3f2a106be0..bd6675ffae 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGaussRadau.cpp @@ -106,7 +106,7 @@ LegendreGaussRadau::getFlattenedVariableInfo() const { for (int i = 0; i < N; ++i) { info.order.push_back({states, igrid + i}); } - if (m_solver.getInterpolateControlMeshInteriorPoints() && imesh == 0) { + if (imesh == 0) { for (int i = 1; i < N; ++i) { info.order.push_back({controls, igrid + i}); } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp index 38110a5e85..850cf8ee72 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.cpp @@ -50,8 +50,7 @@ Iterate Iterate::repmatParameters(int numPoints) { it.variables.at(Var::final_time) = casadi::DM::repmat( it.variables.at(Var::final_time)(0), 1, numPoints); it.variables.at(Var::parameters) = casadi::DM::repmat( - it.variables.at(Var::parameters)(casadi::Slice(), 0), - it.variables.at(Var::parameters).size1(), numPoints); + it.variables.at(Var::parameters)(casadi::Slice(), 0), 1, numPoints); return it; } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h index ec900a024e..f118928540 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h @@ -478,25 +478,27 @@ class Problem { pointsForSparsityDetection); } - if (getEnforceConstraintDerivatives() && - getNumKinematicConstraintEquations()) { + if (getNumKinematicConstraintEquations()) { mutThis->m_kinematicConstraintsFunc = OpenSim::make_unique(); mutThis->m_kinematicConstraintsFunc->constructFunction(this, - "kinematic_constraints", finiteDiffScheme, - pointsForSparsityDetection); - if (isKinematicConstraintMethodBordalba2023()) { - mutThis->m_stateProjectionFunc = - OpenSim::make_unique(); - mutThis->m_stateProjectionFunc->constructFunction(this, - "state_projection", finiteDiffScheme, - pointsForSparsityDetection); - } else { - mutThis->m_velocityCorrectionFunc = - OpenSim::make_unique(); - mutThis->m_velocityCorrectionFunc->constructFunction(this, - "velocity_correction", finiteDiffScheme, - pointsForSparsityDetection); + "kinematic_constraints", finiteDiffScheme, + pointsForSparsityDetection); + + if (getEnforceConstraintDerivatives()) { + if (isKinematicConstraintMethodBordalba2023()) { + mutThis->m_stateProjectionFunc = + OpenSim::make_unique(); + mutThis->m_stateProjectionFunc->constructFunction(this, + "state_projection", finiteDiffScheme, + pointsForSparsityDetection); + } else { + mutThis->m_velocityCorrectionFunc = + OpenSim::make_unique(); + mutThis->m_velocityCorrectionFunc->constructFunction(this, + "velocity_correction", finiteDiffScheme, + pointsForSparsityDetection); + } } } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h index 37464a0505..435c8de38a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h @@ -117,37 +117,6 @@ class Solver { m_stateProjectionWeight = weight; } - /// Whether or not to constrain control values interior to the mesh interval - /// by linearly interpolating control values from mesh interval endpoints. - /// - /// @note For Hermite-Simpson collocation, this applies to the time point - /// at the midpoint of the mesh interval. For Legendre-Gauss and - /// Legendre-Gauss-Radau collocation, this applies to all time points - /// in the interior of the mesh interval. - void setInterpolateControlMeshInteriorPoints(bool tf) { - m_interpolateControlMeshInteriorPoints = tf; - } - /// @copydoc setInterpolateControlMidpoints() - bool getInterpolateControlMeshInteriorPoints() const { - return m_interpolateControlMeshInteriorPoints; - } - - /// Whether or not to enforce path constraints at points interior to the - /// mesh interval. - /// - /// @note For Hermite-Simpson collocation, this applies to the time point - /// at the midpoint of the mesh interval. For Legendre-Gauss and - /// Legendre-Gauss-Radau collocation, this applies to all time points - /// in the interior of the mesh interval. - /// @note Does not apply to implicit dynamics residuals, as these are - /// always enforced at mesh interval midpoints. - void setEnforcePathConstraintMeshInteriorPoints(bool tf) { - m_enforcePathConstraintMeshInteriorPoints = tf; - } - bool getEnforcePathConstraintMeshInteriorPoints() const { - return m_enforcePathConstraintMeshInteriorPoints; - } - void setOptimSolver(std::string optimSolver) { m_optimSolver = std::move(optimSolver); } @@ -225,8 +194,7 @@ class Solver { double m_implicitAuxiliaryDerivativesWeight = 1.0; bool m_minimizeStateProjection = true; double m_stateProjectionWeight = 1e-6; - bool m_interpolateControlMeshInteriorPoints = true; - bool m_enforcePathConstraintMeshInteriorPoints = false; + // bool m_enforcePathConstraintMeshInteriorPoints = false; Bounds m_implicitMultibodyAccelerationBounds; Bounds m_implicitAuxiliaryDerivativeBounds; std::string m_finite_difference_scheme = "central"; diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 72824c729e..df2a1d8a0f 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -119,17 +119,18 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } m_numControlPoints = static_cast(controlIndicesVector.size()); - m_numDynamicsPoints = m_solver.getInterpolateControlMeshInteriorPoints() ? - m_numControlPoints : m_numGridPoints; + m_numUDotErrorPoints = m_problem.isKinematicConstraintMethodBordalba2023() + ? m_numControlPoints + : m_numMeshPoints; // Dynamics constraints. m_numConstraints = m_numDefectsPerMeshInterval * m_numMeshIntervals + - m_numMultibodyResiduals * m_numDynamicsPoints + - m_numAuxiliaryResiduals * m_numDynamicsPoints + + m_numMultibodyResiduals * m_numControlPoints + + m_numAuxiliaryResiduals * m_numControlPoints + m_problem.getNumQErr() * m_numMeshPoints + m_problem.getNumUErr() * m_numMeshPoints + - m_problem.getNumUDotErr() * m_numDynamicsPoints + + m_problem.getNumUDotErr() * m_numUDotErrorPoints + m_problem.getNumProjectionConstraintEquations() * m_numMeshIntervals; // Time constraints. @@ -145,9 +146,7 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } // Path constraints. m_constraints.path.resize(m_problem.getPathConstraintInfos().size()); - m_numPathConstraintPoints = - m_solver.getEnforcePathConstraintMeshInteriorPoints() - ? m_numGridPoints : m_numMeshPoints; + m_numPathConstraintPoints = m_numControlPoints; for (int ipc = 0; ipc < (int)m_constraints.path.size(); ++ipc) { const auto& info = m_problem.getPathConstraintInfos()[ipc]; m_numConstraints += info.size() * m_numPathConstraintPoints; @@ -187,13 +186,14 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_meshIndices = makeTimeIndices(meshIndicesVector); m_meshInteriorIndices = makeTimeIndices(meshInteriorIndicesVector); - m_pathConstraintIndices = - m_solver.getEnforcePathConstraintMeshInteriorPoints() - ? makeTimeIndices(gridIndicesVector) - : makeTimeIndices(meshIndicesVector); m_controlIndices = makeTimeIndices(controlIndicesVector); - m_dynamicsIndices = m_solver.getInterpolateControlMeshInteriorPoints() ? - m_controlIndices : m_gridIndices; + m_pathConstraintIndices = m_controlIndices; + // m_solver.getEnforcePathConstraintMeshInteriorPoints() + // ? makeTimeIndices(gridIndicesVector) + // : makeTimeIndices(meshIndicesVector); + m_udotErrIndices = m_problem.isKinematicConstraintMethodBordalba2023() + ? m_controlIndices + : m_meshIndices; m_projectionStateIndices = makeTimeIndices(projectionStateIndicesVector); m_notProjectionStateIndices = makeTimeIndices(notProjectionStateIndicesVector); @@ -228,38 +228,26 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, MX::sym(fmt::format("states_{}", igrid), m_problem.getNumStates(), 1)); - if (m_solver.getInterpolateControlMeshInteriorPoints()) { - auto it = std::find(controlIndicesVector.begin(), - controlIndicesVector.end(), igrid); - if (it != controlIndicesVector.end()) { - m_scaledVectorVars[controls].push_back( - MX::sym(fmt::format("controls_{}", igrid), - m_problem.getNumControls(), 1)); - m_scaledVectorVars[multipliers].push_back( - MX::sym(fmt::format("multipliers_{}", igrid), - m_problem.getNumMultipliers(), 1)); - m_scaledVectorVars[derivatives].push_back( - MX::sym(fmt::format("derivatives_{}", igrid), - m_problem.getNumDerivatives(), 1)); - - } else { - m_scaledVectorVars[controls].push_back( - MX(m_problem.getNumControls(), 1)); - m_scaledVectorVars[multipliers].push_back( - MX(m_problem.getNumMultipliers(), 1)); - m_scaledVectorVars[derivatives].push_back( - MX(m_problem.getNumDerivatives(), 1)); - } + auto it = std::find(controlIndicesVector.begin(), + controlIndicesVector.end(), igrid); + if (it != controlIndicesVector.end()) { + m_scaledVectorVars[controls].push_back( + MX::sym(fmt::format("controls_{}", igrid), + m_problem.getNumControls(), 1)); + m_scaledVectorVars[multipliers].push_back( + MX::sym(fmt::format("multipliers_{}", igrid), + m_problem.getNumMultipliers(), 1)); + m_scaledVectorVars[derivatives].push_back( + MX::sym(fmt::format("derivatives_{}", igrid), + m_problem.getNumDerivatives(), 1)); + } else { m_scaledVectorVars[controls].push_back( - MX::sym(fmt::format("controls_{}", igrid), - m_problem.getNumControls(), 1)); + MX(m_problem.getNumControls(), 1)); m_scaledVectorVars[multipliers].push_back( - MX::sym(fmt::format("multipliers_{}", igrid), - m_problem.getNumMultipliers(), 1)); + MX(m_problem.getNumMultipliers(), 1)); m_scaledVectorVars[derivatives].push_back( - MX::sym(fmt::format("derivatives_{}", igrid), - m_problem.getNumDerivatives(), 1)); + MX(m_problem.getNumDerivatives(), 1)); } } for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { @@ -268,24 +256,15 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, m_numProjectionStates, 1)); } - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - // In the projection method for enforcing kinematic constraints, the - // slack variables are applied at the mesh points at the end of each - // mesh interval. - for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { - m_scaledVectorVars[slacks].push_back( - MX::sym(fmt::format("slacks_{}", imesh), - m_problem.getNumSlacks(), 1)); - } - } else { - // In the Posa et al. 2016 method for enforcing kinematic constraints, - // the mesh interior points will be the mesh interval midpoints in the - // Hermite-Simpson collocation scheme. - for (int imesh = 0; imesh < m_numMeshInteriorPoints; ++imesh) { - m_scaledVectorVars[slacks].push_back( - MX::sym(fmt::format("slacks_{}", imesh), - m_problem.getNumSlacks(), 1)); - } + // In the Bordalba et al. 2023 method for enforcing kinematic constraints, + // the slack variables are applied at the mesh points at the end of each + // mesh interval. In the Posa et al. 2016 method for enforcing kinematic + // constraints, the mesh interior points will be the mesh interval midpoints + // in the Hermite-Simpson collocation scheme. + for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { + m_scaledVectorVars[slacks].push_back( + MX::sym(fmt::format("slacks_{}", imesh), + m_problem.getNumSlacks(), 1)); } // Concatenate variables. @@ -420,13 +399,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, // Use the bounds from the "true" multibody states to set the bounds for // the projection states. const auto& stateInfos = m_problem.getStateInfos(); - for (int ips = 0; ips < m_numProjectionStates; ++ips) { - const auto& info = stateInfos[ips]; + for (int iproj = 0; iproj < m_numProjectionStates; ++iproj) { + const auto& info = stateInfos[iproj]; setVariableBounds( - projection_states, ips, Slice(0, m_numMeshIntervals - 1), + projection_states, iproj, Slice(0, m_numMeshIntervals - 1), info.bounds); - setVariableBounds(projection_states, ips, -1, info.finalBounds); - setVariableScaling(projection_states, ips, Slice(), info.bounds); + setVariableBounds(projection_states, iproj, -1, info.finalBounds); + setVariableScaling(projection_states, iproj, Slice(), info.bounds); } } { @@ -524,27 +503,26 @@ void Transcription::transcribe() { // Initialize memory for implicit multibody residuals. // --------------------------------------------------- m_constraints.multibody_residuals = MX(casadi::Sparsity::dense( - m_numMultibodyResiduals, m_numDynamicsPoints)); + m_numMultibodyResiduals, m_numControlPoints)); m_constraintsLowerBounds.multibody_residuals = - DM::zeros(m_numMultibodyResiduals, m_numDynamicsPoints); + DM::zeros(m_numMultibodyResiduals, m_numControlPoints); m_constraintsUpperBounds.multibody_residuals = - DM::zeros(m_numMultibodyResiduals, m_numDynamicsPoints); + DM::zeros(m_numMultibodyResiduals, m_numControlPoints); // Initialize memory for implicit auxiliary residuals. // --------------------------------------------------- m_constraints.auxiliary_residuals = MX(casadi::Sparsity::dense( - m_numAuxiliaryResiduals, m_numDynamicsPoints)); + m_numAuxiliaryResiduals, m_numControlPoints)); m_constraintsLowerBounds.auxiliary_residuals = - DM::zeros(m_numAuxiliaryResiduals, m_numDynamicsPoints); + DM::zeros(m_numAuxiliaryResiduals, m_numControlPoints); m_constraintsUpperBounds.auxiliary_residuals = - DM::zeros(m_numAuxiliaryResiduals, m_numDynamicsPoints); + DM::zeros(m_numAuxiliaryResiduals, m_numControlPoints); // Initialize memory for kinematic constraints. // -------------------------------------------- int nqerr = m_problem.getNumQErr(); int nuerr = m_problem.getNumUErr(); int nudoterr = m_problem.getNumUDotErr(); - int nkc = m_problem.getNumKinematicConstraintEquations(); const auto& kcBounds = m_problem.getKinematicConstraintBounds(); m_constraints.kinematic = MX(casadi::Sparsity::dense( @@ -555,11 +533,11 @@ void Transcription::transcribe() { kcBounds.upper, nqerr + nuerr, m_numMeshPoints); m_constraints.kinematic_udoterr = MX(casadi::Sparsity::dense( - nudoterr, m_numDynamicsPoints)); + nudoterr, m_numUDotErrorPoints)); m_constraintsLowerBounds.kinematic_udoterr = casadi::DM::repmat( - kcBounds.lower, nudoterr, m_numDynamicsPoints); + kcBounds.lower, nudoterr, m_numUDotErrorPoints); m_constraintsUpperBounds.kinematic_udoterr = casadi::DM::repmat( - kcBounds.upper, nudoterr, m_numDynamicsPoints); + kcBounds.upper, nudoterr, m_numUDotErrorPoints); // Initialize memory for projection constraints. // --------------------------------------------- @@ -585,8 +563,7 @@ void Transcription::transcribe() { // kinematic constraints // --------------------- if (m_problem.getNumKinematicConstraintEquations() && - !m_problem.isPrescribedKinematics() && - m_problem.getEnforceConstraintDerivatives()) { + !m_problem.isPrescribedKinematics()) { OPENSIM_THROW_IF(!m_problem.isKinematicConstraintMethodBordalba2023() && m_solver.getTranscriptionScheme() != "hermite-simpson", @@ -602,10 +579,11 @@ void Transcription::transcribe() { {multibody_states}, m_meshIndices); m_constraints.kinematic = out.at(0); - // projection constraints - // ---------------------- + // velocity correction + // ------------------- if (m_solver.getTranscriptionScheme() == "hermite-simpson" && - !m_problem.isKinematicConstraintMethodBordalba2023()) { + !m_problem.isKinematicConstraintMethodBordalba2023() && + m_problem.getEnforceConstraintDerivatives()) { // In Hermite-Simpson, we must compute a velocity correction at all // mesh interval midpoints and update qdot. // See MocoCasADiVelocityCorrection for more details. This function @@ -619,6 +597,8 @@ void Transcription::transcribe() { m_xdot(Slice(0, NQ), m_meshInteriorIndices) += u_correction; } + // projection constraints + // ---------------------- if (m_numProjectionStates) { const auto projectionOut = evalOnTrajectory( m_problem.getStateProjection(), @@ -654,16 +634,13 @@ void Transcription::transcribe() { std::vector inputs{states, controls, multipliers, derivatives}; const auto out = evalOnTrajectory(m_problem.getImplicitMultibodySystem(), inputs, m_gridIndices); - m_constraints.multibody_residuals = - out.at(0)(Slice(), m_dynamicsIndices); + m_constraints.multibody_residuals = out.at(0)(Slice(), m_controlIndices); // zdot. m_xdot(Slice(NQ + NU, NS), Slice()) = out.at(1); - m_constraints.auxiliary_residuals = - out.at(2)(Slice(), m_dynamicsIndices); - m_constraints.kinematic_udoterr = - out.at(3)(Slice(), m_dynamicsIndices); + m_constraints.auxiliary_residuals = out.at(2)(Slice(), m_controlIndices); + m_constraints.kinematic_udoterr = out.at(3)(Slice(), m_udotErrIndices); - // Points where the multibody residuals and udoterro depend on the + // Points where the multibody residuals and udoterr depend on the // projection states. if (m_numProjectionStates && !m_projectionStateIndicesForControlIndices.is_empty()) { @@ -698,9 +675,8 @@ void Transcription::transcribe() { inputs, m_gridIndices); m_xdot(Slice(NQ, NQ + NU), Slice()) = out.at(0); m_xdot(Slice(NQ + NU, NS), Slice()) = out.at(1); - m_constraints.auxiliary_residuals = - out.at(2)(Slice(), m_dynamicsIndices); - m_constraints.kinematic_udoterr = out.at(3)(Slice(), m_dynamicsIndices); + m_constraints.auxiliary_residuals = out.at(2)(Slice(), m_controlIndices); + m_constraints.kinematic_udoterr = out.at(3)(Slice(), m_udotErrIndices); // Points where the state derivatives and udoterr depend on the // projection states. @@ -1129,13 +1105,7 @@ Solution Transcription::solve(const Iterate& guessOrig) { printObjectiveBreakdown(solution, objectiveOut[0]); if (!solution.stats.at("success")) { - - // For some reason, nlpResult.at("g") is all 0. So we calculate the - // constraints ourselves. - casadi::Function constraintFunc("constraints", {x}, {flat.constraints}); - casadi::DMVector constraintsOut; - constraintFunc.call(finalVarsDMV, constraintsOut); - printConstraintValues(solution, expandConstraints(constraintsOut[0])); + printConstraintValues(solution, expandConstraints(nlpResult.at("g"))); } return solution; } @@ -1357,9 +1327,12 @@ void Transcription::printConstraintValues(const Iterate& it, printParameterBounds( "Time bounds", time_names, timeValues, timeLower, timeUpper); - printParameterBounds("Parameter bounds", it.parameter_names, - vars.at(parameters)(0), lower.at(parameters)(0), - upper.at(parameters)(0)); + if (!it.parameter_names.empty()) { + printParameterBounds("Parameter bounds", it.parameter_names, + vars.at(parameters)(Slice(), 0), + lower.at(parameters)(Slice(), 0), + upper.at(parameters)(Slice(), 0)); + } // Constraints. // ============ @@ -1409,8 +1382,6 @@ void Transcription::printConstraintValues(const Iterate& it, int numQErr = m_problem.getNumQErr(); int numUErr = m_problem.getNumUErr(); int numUDotErr = m_problem.getNumUDotErr(); - int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? - numQErr + numUErr : numQErr + numUErr + numUDotErr; if (kinconNames.empty()) { ss << " none" << std::endl; } else { @@ -1423,7 +1394,7 @@ void Transcription::printConstraintValues(const Iterate& it, row.resize(1, m_numMeshPoints); { int ikc = 0; - for (int i = 0; i < numKC; ++i) { + for (int i = 0; i < numQErr + numUErr; ++i) { row = constraints.kinematic(i, Slice()); const double L2 = casadi::DM::norm_2(row).scalar(); int argmax; @@ -1440,24 +1411,22 @@ void Transcription::printConstraintValues(const Iterate& it, << std::endl; ++ikc; } - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - for (int iudot = 0; iudot < numUDotErr; ++iudot) { - row = constraints.kinematic_udoterr(iudot, Slice()); - const double L2 = casadi::DM::norm_2(row).scalar(); - int argmax; - double max = calcL1Norm(row, argmax); - const double L1 = max; - const double time_of_max = it.times(argmax).scalar(); + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + row = constraints.kinematic_udoterr(iudot, Slice()); + const double L2 = casadi::DM::norm_2(row).scalar(); + int argmax; + double max = calcL1Norm(row, argmax); + const double L1 = max; + const double time_of_max = it.times(argmax).scalar(); - std::string label = kinconNames.at(ikc); - ss << std::setfill('0') << std::setw(2) << ikc << ":" - << std::setfill(' ') << std::setw(maxNameLength) << label - << spacer << std::setprecision(2) << std::scientific - << std::setw(9) << L2 << spacer << L1 << spacer - << std::setprecision(6) << std::fixed << time_of_max - << std::endl; - ++ikc; - } + std::string label = kinconNames.at(ikc); + ss << std::setfill('0') << std::setw(2) << ikc << ":" + << std::setfill(' ') << std::setw(maxNameLength) << label + << spacer << std::setprecision(2) << std::scientific + << std::setw(9) << L2 << spacer << L1 << spacer + << std::setprecision(6) << std::fixed << time_of_max + << std::endl; + ++ikc; } } ss << "Kinematic constraint values at each mesh point:" @@ -1471,20 +1440,19 @@ void Transcription::printConstraintValues(const Iterate& it, ss << std::setfill('0') << std::setw(3) << imesh << " "; ss.fill(' '); ss << std::setw(9) << it.times(imesh).scalar() << " "; - for (int i = 0; i < numKC; ++i) { + for (int i = 0; i < numQErr + numUErr; ++i) { const auto& value = constraints.kinematic(i, imesh).scalar(); ss << std::setprecision(2) << std::scientific << std::setw(9) << value << " "; } - if (m_problem.isKinematicConstraintMethodBordalba2023()) { - for (int iudot = 0; iudot < numUDotErr; ++iudot) { - const auto& value = - constraints.kinematic_udoterr(iudot, imesh).scalar(); - ss << std::setprecision(2) << std::scientific - << std::setw(9) << value << " "; - } - } + // TODO fix this to print udoterr values correctly + for (int iudot = 0; iudot < numUDotErr; ++iudot) { + const auto& value = + constraints.kinematic_udoterr(iudot, imesh).scalar(); + ss << std::setprecision(2) << std::scientific + << std::setw(9) << value << " "; + } ss << std::endl; } } diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 2b1ec72293..67defd8cf0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -174,7 +174,7 @@ class Transcription { int m_numGapClosingDefectsPerMeshInterval = -1; int m_numPointsPerMeshInterval = -1; int m_numControlPoints = -1; - int m_numDynamicsPoints = -1; + int m_numUDotErrorPoints = -1; int m_numMultibodyResiduals = -1; int m_numAuxiliaryResiduals = -1; int m_numConstraints = -1; @@ -206,7 +206,7 @@ class Transcription { casadi::Matrix m_meshInteriorIndices; casadi::Matrix m_pathConstraintIndices; casadi::Matrix m_controlIndices; - casadi::Matrix m_dynamicsIndices; + casadi::Matrix m_udotErrIndices; casadi::Matrix m_projectionStateIndices; casadi::Matrix m_notProjectionStateIndices; casadi::Matrix m_projectionStateIndicesForControlIndices; @@ -276,11 +276,9 @@ class Transcription { } template void calcInterpolatingControls(Variables& vars) const { - if (m_solver.getInterpolateControlMeshInteriorPoints()) { - calcInterpolatingControlsImpl(vars.at(controls)); - calcInterpolatingControlsImpl(vars.at(multipliers)); - calcInterpolatingControlsImpl(vars.at(derivatives)); - } + calcInterpolatingControlsImpl(vars.at(controls)); + calcInterpolatingControlsImpl(vars.at(multipliers)); + calcInterpolatingControlsImpl(vars.at(derivatives)); } /// Convert the map of variables into a column vector, for passing onto @@ -464,9 +462,7 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int idyn = 0; - auto dynamicsIndices = - m_solver.getInterpolateControlMeshInteriorPoints() ? - createControlIndices() : casadi::DM::ones(1, m_numGridPoints); + auto dynamicsIndices = createControlIndices(); int ng = 0; for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; @@ -482,39 +478,48 @@ class Transcription { ng += m_numDefectsPerMeshInterval - m_numGapClosingDefectsPerMeshInterval; - // Kinematic constraints (qerr and uerr). + // Kinematic constraints. copyColumn(constraints.kinematic, imesh); ng += constraints.kinematic.rows(); + if (!m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(constraints.kinematic_udoterr, imesh); + ng += constraints.kinematic_udoterr.rows(); + } - // Multibody residuals, auxiliary residuals, and acceleration-level - // kinematic constraints (udoterr). + // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { if (dynamicsIndices(igrid + i).scalar() == 1) { - copyColumn(constraints.kinematic_udoterr, idyn); - ng += constraints.kinematic_udoterr.rows(); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(constraints.kinematic_udoterr, idyn); + ng += constraints.kinematic_udoterr.rows(); + } copyColumn(constraints.multibody_residuals, idyn); ng += constraints.multibody_residuals.rows(); copyColumn(constraints.auxiliary_residuals, idyn); ng += constraints.auxiliary_residuals.rows(); - ++idyn; - } - } - - // Path constraints. - if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - for (int i = 0; i < N; ++i) { for (const auto& path : constraints.path) { - copyColumn(path, igrid + i); + copyColumn(path, idyn); ng += path.rows(); } - } - } else { - for (const auto& path : constraints.path) { - copyColumn(path, imesh); - ng += path.rows(); + ++idyn; } } + // Path constraints. + // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + // for (int i = 0; i < N; ++i) { + // for (const auto& path : constraints.path) { + // copyColumn(path, igrid + i); + // ng += path.rows(); + // } + // } + // } else { + // for (const auto& path : constraints.path) { + // copyColumn(path, imesh); + // ng += path.rows(); + // } + // } + // Projection constraints. if (imesh > 0) { copyColumn(constraints.projection, imesh - 1); @@ -528,25 +533,35 @@ class Transcription { // Final grid point. copyColumn(constraints.kinematic, m_numMeshPoints - 1); ng += constraints.kinematic.rows(); - if (dynamicsIndices(m_numGridPoints - 1).scalar() == 1) { - copyColumn(constraints.kinematic_udoterr, idyn); + if (!m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(constraints.kinematic_udoterr, m_numMeshPoints - 1); ng += constraints.kinematic_udoterr.rows(); + } + if (dynamicsIndices(m_numGridPoints - 1).scalar() == 1) { + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(constraints.kinematic_udoterr, idyn); + ng += constraints.kinematic_udoterr.rows(); + } copyColumn(constraints.multibody_residuals, idyn); ng += constraints.multibody_residuals.rows(); copyColumn(constraints.auxiliary_residuals, idyn); ng += constraints.auxiliary_residuals.rows(); - } - if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - for (const auto& path : constraints.path) { - copyColumn(path, m_numGridPoints - 1); - ng += path.rows(); - } - } else { for (const auto& path : constraints.path) { - copyColumn(path, m_numMeshPoints - 1); + copyColumn(path, idyn); ng += path.rows(); } } + // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + // for (const auto& path : constraints.path) { + // copyColumn(path, m_numGridPoints - 1); + // ng += path.rows(); + // } + // } else { + // for (const auto& path : constraints.path) { + // copyColumn(path, m_numMeshPoints - 1); + // ng += path.rows(); + // } + // } copyColumn(constraints.projection, m_numMeshIntervals - 1); ng += constraints.projection.rows(); flat.ng.push_back(ng); @@ -577,10 +592,8 @@ class Transcription { int numQErr = m_problem.getNumQErr(); int numUErr = m_problem.getNumUErr(); int numUDotErr = m_problem.getNumUDotErr(); - int numKC = m_problem.isKinematicConstraintMethodBordalba2023() ? - numQErr + numUErr : numQErr + numUErr + numUDotErr; out.kinematic = init(numQErr + numUErr, m_numMeshPoints); - out.kinematic_udoterr = init(numUDotErr, m_numDynamicsPoints); + out.kinematic_udoterr = init(numUDotErr, m_numUDotErrorPoints); out.projection = init(m_problem.getNumProjectionConstraintEquations(), m_numMeshIntervals); out.endpoint.resize(m_problem.getEndpointConstraintInfos().size()); @@ -611,9 +624,7 @@ class Transcription { // Constraints for each mesh interval. int N = m_numPointsPerMeshInterval - 1; int idyn = 0; - auto dynamicsIndices = - m_solver.getInterpolateControlMeshInteriorPoints() ? - createControlIndices() : casadi::DM::ones(1, m_numGridPoints); + auto dynamicsIndices = createControlIndices(); for (int imesh = 0; imesh < m_numMeshIntervals; ++imesh) { int igrid = imesh * N; @@ -626,32 +637,39 @@ class Transcription { // Defect constraints. copyColumn(out.defects, imesh); - // Kinematic constraints (qerr and uerr). + // Kinematic constraints. copyColumn(out.kinematic, imesh); + if (!m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(out.kinematic_udoterr, imesh); + } - // Multibody residuals, auxiliary residuals, and acceleration-level - // kinematic constraints (udoterr). + // Multibody and auxiliary residuals. for (int i = 0; i < N; ++i) { if (dynamicsIndices(igrid + i).scalar() == 1) { - copyColumn(out.kinematic_udoterr, igrid + i); - copyColumn(out.multibody_residuals, igrid + i); - copyColumn(out.auxiliary_residuals, igrid + i); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(out.kinematic_udoterr, idyn); + } + copyColumn(out.multibody_residuals, idyn); + copyColumn(out.auxiliary_residuals, idyn); + for (auto& path : out.path) { + copyColumn(path, idyn); + } ++idyn; } } // Path constraints. - if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - for (int i = 0; i < N; ++i) { - for (auto& path : out.path) { - copyColumn(path, igrid + i); - } - } - } else { - for (auto& path : out.path) { - copyColumn(path, imesh); - } - } + // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + // for (int i = 0; i < N; ++i) { + // for (auto& path : out.path) { + // copyColumn(path, igrid + i); + // } + // } + // } else { + // for (auto& path : out.path) { + // copyColumn(path, imesh); + // } + // } // Projection constraints. if (imesh > 0) { @@ -661,20 +679,28 @@ class Transcription { // Final grid point. copyColumn(out.kinematic, m_numMeshPoints - 1); + if (!m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(out.kinematic_udoterr, m_numMeshPoints - 1); + } if (dynamicsIndices(m_numGridPoints - 1).scalar() == 1) { - copyColumn(out.kinematic_udoterr, idyn); + if (m_problem.isKinematicConstraintMethodBordalba2023()) { + copyColumn(out.kinematic_udoterr, idyn); + } copyColumn(out.multibody_residuals, idyn); copyColumn(out.auxiliary_residuals, idyn); - } - if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - for (auto& path : out.path) { - copyColumn(path, m_numGridPoints - 1); - } - } else { for (auto& path : out.path) { - copyColumn(path, m_numMeshPoints - 1); + copyColumn(path, idyn); } } + // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { + // for (auto& path : out.path) { + // copyColumn(path, m_numGridPoints - 1); + // } + // } else { + // for (auto& path : out.path) { + // copyColumn(path, m_numMeshPoints - 1); + // } + // } copyColumn(out.projection, m_numMeshIntervals - 1); OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index df1737ad0a..d55380d862 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -340,10 +340,6 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( casSolver->setStateProjectionWeight(get_state_projection_distance_weight()); casSolver->setOptimSolver(get_optim_solver()); - casSolver->setInterpolateControlMeshInteriorPoints( - get_interpolate_control_mesh_interior_points()); - casSolver->setEnforcePathConstraintMeshInteriorPoints( - get_enforce_path_constraint_mesh_interior_points()); if (casProblem.getJarSize() > 1) { casSolver->setParallelism("thread", casProblem.getJarSize()); } diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.h b/OpenSim/Moco/MocoDirectCollocationSolver.h index d671d17b15..e26a8889ec 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.h +++ b/OpenSim/Moco/MocoDirectCollocationSolver.h @@ -32,6 +32,8 @@ The best resource for learning about direct collocation is the Betts Betts, John T. Practical methods for optimal control and estimation using nonlinear programming. Vol. 19. Siam, 2010. +TODO update these docs to reflect the kinematic constraint changes. + MocoDirectCollocationSolver =========================== Transcription scheme diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index 190533574b..7ce251be57 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -87,263 +87,269 @@ TEMPLATE_TEST_CASE( auto isTendonDynamicsExplicit = GENERATE(true, false); auto transcription_scheme = GENERATE(as{}, "hermite-simpson", "legendre-gauss-3", "legendre-gauss-radau-3"); - - // CAPTURE prints the current value of these variables to the console. - CAPTURE(ignoreActivationDynamics); - CAPTURE(ignoreTendonCompliance); - CAPTURE(isTendonDynamicsExplicit); - - const double optimalFiberLength = 0.1; - const double tendonSlackLength = 0.05; - SimTK::Real initHeight = 0.165; - SimTK::Real finalHeight = 0.155; - SimTK::Real initSpeed = 0; - SimTK::Real finalSpeed = 0; - MocoBounds heightBounds(0.14, 0.17); - MocoBounds speedBounds(-10, 10); - MocoBounds actuBounds(0.1, 1); - MocoBounds normTendonBounds(0.1, 2); - - Model model = createHangingMuscleModel(optimalFiberLength, - tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, - isTendonDynamicsExplicit); - - SimTK::State state = model.initSystem(); - - // Minimum time trajectory optimization. - // ------------------------------------- - const auto svn = model.getStateVariableNames(); - MocoSolution solutionTrajOpt; - std::string solutionFilename; - { - MocoStudy study; - MocoProblem& problem = study.updProblem(); - problem.setModelAsCopy(model); - problem.setTimeBounds(0, 0.5); - problem.setStateInfo( - "/joint/height/value", heightBounds, initHeight, finalHeight); - problem.setStateInfo( - "/joint/height/speed", speedBounds, initSpeed, finalSpeed); - problem.setControlInfo("/forceset/muscle", - actuBounds, actuBounds.getLower()); - if (!ignoreActivationDynamics) { - problem.setStateInfo("/forceset/muscle/activation", - actuBounds, actuBounds.getLower()); - } - if (!ignoreTendonCompliance) { - problem.setStateInfo("/forceset/muscle/normalized_tendon_force", - normTendonBounds, normTendonBounds.getLower()); - } - - problem.addGoal("effort"); - - auto& solver = study.initSolver(); - solver.set_num_mesh_intervals(30); - solver.set_multibody_dynamics_mode("explicit"); - solver.set_optim_convergence_tolerance(1e-4); - solver.set_optim_constraint_tolerance(1e-4); - solver.set_transcription_scheme(transcription_scheme); - - solutionTrajOpt = study.solve(); - solutionFilename = "testDeGrooteFregly2016Muscle_solution"; - if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; - if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; - - std::vector outputPaths; - outputPaths.emplace_back(".*tendon_force.*"); - outputPaths.emplace_back(".*fiber_force_along_tendon.*"); - outputPaths.emplace_back(".*length.*"); - outputPaths.emplace_back(".*velocity.*"); - auto table = study.analyze(solutionTrajOpt, outputPaths); - //STOFileAdapter::write(table, solutionFilename + "_outputs.sto"); - - solutionFilename += ".sto"; - solutionTrajOpt.write(solutionFilename); - - // Check that the muscle and tendon are in equilibrium. - const auto& muscle = model.getComponent( - "/forceset/muscle"); - auto statesTable = solutionTrajOpt.exportToStatesTable(); - const auto& activeFiberForceAlongTendon = table.getDependentColumn( - "/forceset/muscle|active_fiber_force_along_tendon"); - const auto& passiveFiberForceAlongTendon = table.getDependentColumn( - "/forceset/muscle|passive_fiber_force_along_tendon"); - const auto& tendonForce = table.getDependentColumn( - "/forceset/muscle|tendon_force"); - SimTK::Vector equilibriumResidual((int)table.getNumRows(), 0.0); - for (int i = 0; i < (int)table.getNumRows(); ++i) { - const double fiberForceAlongTendon = - activeFiberForceAlongTendon[i] + - passiveFiberForceAlongTendon[i]; - equilibriumResidual[i] = (tendonForce[i] - fiberForceAlongTendon) / - muscle.getMaxIsometricForce(); - } - CHECK(equilibriumResidual.normRMS() < 1e-3); - } - - // Perform time stepping forward simulation using optimized controls. - // ------------------------------------------------------------------ - // See if we end up at the correct final state. - { - // We need to temporarily switch the muscle in the model to explicit - // tendon compliance dynamics mode to perform time stepping. - auto* mutableDGFMuscle = dynamic_cast( - &model.updComponent("forceset/muscle")); - if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { - mutableDGFMuscle->set_tendon_compliance_dynamics_mode("explicit"); - } - const auto trajSim = - simulateTrajectoryWithTimeStepping(solutionTrajOpt, model, 1e-6); - std::string trajFilename = "testDeGrooteFregly2016Muscle_timestepping"; - if (!ignoreActivationDynamics) trajFilename += "_actdyn"; - if (ignoreTendonCompliance) trajFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) trajFilename += "_exptendyn"; - trajFilename += ".sto"; - //trajSim.write(trajFilename); - - const double error = trajSim.compareContinuousVariablesRMS( - solutionTrajOpt, {{"states", {}}, {"controls", {}}}); - CHECK(error < 0.1); - if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { - mutableDGFMuscle->set_tendon_compliance_dynamics_mode("implicit"); + std::string section = fmt::format( + "actdyn={}, tendyn={}, exptendyn={}, scheme={}", + !ignoreActivationDynamics, !ignoreTendonCompliance, + isTendonDynamicsExplicit, transcription_scheme); + DYNAMIC_SECTION(section) { + + // CAPTURE prints the current value of these variables to the console. + CAPTURE(ignoreActivationDynamics); + CAPTURE(ignoreTendonCompliance); + CAPTURE(isTendonDynamicsExplicit); + + const double optimalFiberLength = 0.1; + const double tendonSlackLength = 0.05; + SimTK::Real initHeight = 0.165; + SimTK::Real finalHeight = 0.155; + SimTK::Real initSpeed = 0; + SimTK::Real finalSpeed = 0; + MocoBounds heightBounds(0.14, 0.17); + MocoBounds speedBounds(-10, 10); + MocoBounds actuBounds(0.1, 1); + MocoBounds normTendonBounds(0.1, 2); + + Model model = createHangingMuscleModel(optimalFiberLength, + tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, + isTendonDynamicsExplicit); + + SimTK::State state = model.initSystem(); + + // Minimum time trajectory optimization. + // ------------------------------------- + const auto svn = model.getStateVariableNames(); + MocoSolution solutionTrajOpt; + std::string solutionFilename; + { + MocoStudy study; + MocoProblem& problem = study.updProblem(); + problem.setModelAsCopy(model); + problem.setTimeBounds(0, 0.5); + problem.setStateInfo( + "/joint/height/value", heightBounds, initHeight, finalHeight); + problem.setStateInfo( + "/joint/height/speed", speedBounds, initSpeed, finalSpeed); + problem.setControlInfo("/forceset/muscle", + actuBounds, actuBounds.getLower()); + if (!ignoreActivationDynamics) { + problem.setStateInfo("/forceset/muscle/activation", + actuBounds, actuBounds.getLower()); + } + if (!ignoreTendonCompliance) { + problem.setStateInfo("/forceset/muscle/normalized_tendon_force", + normTendonBounds, normTendonBounds.getLower()); + } + + problem.addGoal("effort"); + + auto& solver = study.initSolver(); + solver.set_num_mesh_intervals(30); + solver.set_multibody_dynamics_mode("explicit"); + solver.set_optim_convergence_tolerance(1e-4); + solver.set_optim_constraint_tolerance(1e-4); + solver.set_transcription_scheme(transcription_scheme); + + solutionTrajOpt = study.solve(); + solutionFilename = "testDeGrooteFregly2016Muscle_solution"; + if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; + if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; + + std::vector outputPaths; + outputPaths.emplace_back(".*tendon_force.*"); + outputPaths.emplace_back(".*fiber_force_along_tendon.*"); + outputPaths.emplace_back(".*length.*"); + outputPaths.emplace_back(".*velocity.*"); + auto table = study.analyze(solutionTrajOpt, outputPaths); + //STOFileAdapter::write(table, solutionFilename + "_outputs.sto"); + + solutionFilename += ".sto"; + solutionTrajOpt.write(solutionFilename); + + // Check that the muscle and tendon are in equilibrium. + const auto& muscle = model.getComponent( + "/forceset/muscle"); + auto statesTable = solutionTrajOpt.exportToStatesTable(); + const auto& activeFiberForceAlongTendon = table.getDependentColumn( + "/forceset/muscle|active_fiber_force_along_tendon"); + const auto& passiveFiberForceAlongTendon = table.getDependentColumn( + "/forceset/muscle|passive_fiber_force_along_tendon"); + const auto& tendonForce = table.getDependentColumn( + "/forceset/muscle|tendon_force"); + SimTK::Vector equilibriumResidual((int)table.getNumRows(), 0.0); + for (int i = 0; i < (int)table.getNumRows(); ++i) { + const double fiberForceAlongTendon = + activeFiberForceAlongTendon[i] + + passiveFiberForceAlongTendon[i]; + equilibriumResidual[i] = (tendonForce[i] - fiberForceAlongTendon) / + muscle.getMaxIsometricForce(); + } + CHECK(equilibriumResidual.normRMS() < 1e-3); } - } - // Solve problem again using CMC. - // ------------------------------ - // See if we get the correct muscle activity. - if (!ignoreActivationDynamics) { - Model modelCMC = - createHangingMuscleModel(optimalFiberLength, tendonSlackLength, - ignoreActivationDynamics, ignoreTendonCompliance, true); - - // Need a reserve for CMC to solve. - auto* actu = new CoordinateActuator(); - actu->setName("actuator"); - actu->setOptimalForce(0.1); - actu->setMinControl(-100); - actu->setMaxControl(100); - actu->setCoordinate(&modelCMC.getCoordinateSet().get(0)); - modelCMC.addForce(actu); - modelCMC.finalizeConnections(); - - // Run CMC - // ------- - CMCTool cmc; - std::string cmcFilename = "testDeGrooteFregly2016Muscle_cmc"; - cmc.setResultsDir(cmcFilename); - if (!ignoreActivationDynamics) cmcFilename += "_actdyn"; - if (ignoreTendonCompliance) cmcFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) cmcFilename += "_exptendyn"; - cmc.setName(cmcFilename); - cmc.setModel(modelCMC); - cmc.setInitialTime(0); - const double finalTime = - solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; - cmc.setFinalTime(finalTime); - cmc.setDesiredKinematicsFileName(solutionFilename); - cmc.setTaskSetFileName("hanging_muscle_cmc_tasks.xml"); - cmc.setSolveForEquilibrium(false); - cmc.setTimeWindow(0.01); - cmc.setUseFastTarget(true); - cmc.run(); - - // Create a MocoTrajectory from the CMC solution. - TimeSeriesTable cmcStates; - std::vector stateColumnLabels; - cmcStates = TimeSeriesTable("testDeGrooteFregly2016Muscle_cmc/" + - cmcFilename + "_states.sto"); - stateColumnLabels = cmcStates.getColumnLabels(); - - TimeSeriesTable cmcControls("testDeGrooteFregly2016Muscle_cmc/" + - cmcFilename + "_controls.sto"); - const auto& stdTime = cmcControls.getIndependentColumn(); - SimTK::Vector time((int)stdTime.size(), stdTime.data()); - std::vector controlNames{"/forceset/muscle"}; - MocoTrajectory cmcTraj(time, stateColumnLabels, controlNames, {}, {}, - cmcStates.getMatrix(), - cmcControls.getMatrixBlock(0, 0, cmcControls.getNumRows(), 1), - SimTK::Matrix(), SimTK::RowVector()); - //cmcTraj.write( - // "testDeGrooteFregly2016Muscle_cmc/" + cmcFilename + ".sto"); - - // Compare CMC solution to the Moco solution. - std::vector states; - states.push_back("/forceset/muscle/activation"); - if (!ignoreTendonCompliance) { - states.push_back("/forceset/muscle/normalized_tendon_force"); + // Perform time stepping forward simulation using optimized controls. + // ------------------------------------------------------------------ + // See if we end up at the correct final state. + { + // We need to temporarily switch the muscle in the model to explicit + // tendon compliance dynamics mode to perform time stepping. + auto* mutableDGFMuscle = dynamic_cast( + &model.updComponent("forceset/muscle")); + if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { + mutableDGFMuscle->set_tendon_compliance_dynamics_mode("explicit"); + } + const auto trajSim = + simulateTrajectoryWithTimeStepping(solutionTrajOpt, model, 1e-6); + std::string trajFilename = "testDeGrooteFregly2016Muscle_timestepping"; + if (!ignoreActivationDynamics) trajFilename += "_actdyn"; + if (ignoreTendonCompliance) trajFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) trajFilename += "_exptendyn"; + trajFilename += ".sto"; + //trajSim.write(trajFilename); + + const double error = trajSim.compareContinuousVariablesRMS( + solutionTrajOpt, {{"states", {}}, {"controls", {}}}); + CHECK(error < 0.1); + if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { + mutableDGFMuscle->set_tendon_compliance_dynamics_mode("implicit"); + } } - // Only compare states, since CMC controls are spiky. Only need a rough - // comparison here, just confirming that CMC works with the muscle model. - const double error = cmcTraj.compareContinuousVariablesRMS( - solutionTrajOpt, {{"states", states}}); - CHECK(error < 0.5); - } - // Track the kinematics from the trajectory optimization. - // ------------------------------------------------------ - // We will try to recover muscle activity. - if (!isTendonDynamicsExplicit) { - std::cout << "Tracking the trajectory optimization coordinate solution." - << std::endl; - MocoStudy study; - MocoProblem& problem = study.updProblem(); - problem.setModelAsCopy(model); - // Using an equality constraint for the time bounds was essential for - // recovering the correct excitation. - const double finalTime = - solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; - problem.setTimeBounds(0, finalTime); - problem.setStateInfo( - "/joint/height/value", heightBounds, initHeight, finalHeight); - problem.setStateInfo( - "/joint/height/speed", speedBounds, initSpeed, finalSpeed); - problem.setControlInfo("/forceset/muscle", - actuBounds, actuBounds.getLower()); + // Solve problem again using CMC. + // ------------------------------ + // See if we get the correct muscle activity. if (!ignoreActivationDynamics) { - problem.setStateInfo("/forceset/muscle/activation", - actuBounds, actuBounds.getLower()); - } - if (!ignoreTendonCompliance) { - problem.setStateInfo("/forceset/muscle/normalized_tendon_force", - normTendonBounds, normTendonBounds.getLower()); + Model modelCMC = + createHangingMuscleModel(optimalFiberLength, tendonSlackLength, + ignoreActivationDynamics, ignoreTendonCompliance, true); + + // Need a reserve for CMC to solve. + auto* actu = new CoordinateActuator(); + actu->setName("actuator"); + actu->setOptimalForce(0.1); + actu->setMinControl(-100); + actu->setMaxControl(100); + actu->setCoordinate(&modelCMC.getCoordinateSet().get(0)); + modelCMC.addForce(actu); + modelCMC.finalizeConnections(); + + // Run CMC + // ------- + CMCTool cmc; + std::string cmcFilename = "testDeGrooteFregly2016Muscle_cmc"; + cmc.setResultsDir(cmcFilename); + if (!ignoreActivationDynamics) cmcFilename += "_actdyn"; + if (ignoreTendonCompliance) cmcFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) cmcFilename += "_exptendyn"; + cmc.setName(cmcFilename); + cmc.setModel(modelCMC); + cmc.setInitialTime(0); + const double finalTime = + solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; + cmc.setFinalTime(finalTime); + cmc.setDesiredKinematicsFileName(solutionFilename); + cmc.setTaskSetFileName("hanging_muscle_cmc_tasks.xml"); + cmc.setSolveForEquilibrium(false); + cmc.setTimeWindow(0.01); + cmc.setUseFastTarget(true); + cmc.run(); + + // Create a MocoTrajectory from the CMC solution. + TimeSeriesTable cmcStates; + std::vector stateColumnLabels; + cmcStates = TimeSeriesTable("testDeGrooteFregly2016Muscle_cmc/" + + cmcFilename + "_states.sto"); + stateColumnLabels = cmcStates.getColumnLabels(); + + TimeSeriesTable cmcControls("testDeGrooteFregly2016Muscle_cmc/" + + cmcFilename + "_controls.sto"); + const auto& stdTime = cmcControls.getIndependentColumn(); + SimTK::Vector time((int)stdTime.size(), stdTime.data()); + std::vector controlNames{"/forceset/muscle"}; + MocoTrajectory cmcTraj(time, stateColumnLabels, controlNames, {}, {}, + cmcStates.getMatrix(), + cmcControls.getMatrixBlock(0, 0, cmcControls.getNumRows(), 1), + SimTK::Matrix(), SimTK::RowVector()); + //cmcTraj.write( + // "testDeGrooteFregly2016Muscle_cmc/" + cmcFilename + ".sto"); + + // Compare CMC solution to the Moco solution. + std::vector states; + states.push_back("/forceset/muscle/activation"); + if (!ignoreTendonCompliance) { + states.push_back("/forceset/muscle/normalized_tendon_force"); + } + // Only compare states, since CMC controls are spiky. Only need a rough + // comparison here, just confirming that CMC works with the muscle model. + const double error = cmcTraj.compareContinuousVariablesRMS( + solutionTrajOpt, {{"states", states}}); + CHECK(error < 0.5); } - auto* tracking = problem.addGoal("tracking"); - - auto states = solutionTrajOpt.exportToStatesTable(); - TimeSeriesTable ref(states.getIndependentColumn()); - ref.addTableMetaData("inDegrees", std::string("no")); - ref.appendColumn("/joint/height/value", - states.getDependentColumn("/joint/height/value")); - // Tracking speed has a huge effect on getting a good solution for the - // control signal. - ref.appendColumn("/joint/height/speed", - states.getDependentColumn("/joint/height/speed")); - // Tracking joint/height/speed slightly increases the - // iterations to converge, and tracking activation cuts the iterations - // in half. - tracking->setReference(ref); - tracking->setAllowUnusedReferences(true); - - // Need a low-weighted effort cost so the problem is well-conditioned. - problem.addGoal("effort", 1e-3); - - auto& solver = study.initSolver(); - solver.set_num_mesh_intervals(30); - - MocoSolution solutionTrack = study.solve(); - std::string solutionFilename = - "testDeGrooteFregly2016Muscle_track_solution"; - if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; - if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; - solutionFilename += ".sto"; - //solutionTrack.write(solutionFilename); - double error = solutionTrack.compareContinuousVariablesRMS( - solutionTrajOpt, {{"states", {}}, {"controls", {}}}); - CHECK(error < 0.01); + // Track the kinematics from the trajectory optimization. + // ------------------------------------------------------ + // We will try to recover muscle activity. + if (!isTendonDynamicsExplicit) { + std::cout << "Tracking the trajectory optimization coordinate solution." + << std::endl; + MocoStudy study; + MocoProblem& problem = study.updProblem(); + problem.setModelAsCopy(model); + // Using an equality constraint for the time bounds was essential for + // recovering the correct excitation. + const double finalTime = + solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; + problem.setTimeBounds(0, finalTime); + problem.setStateInfo( + "/joint/height/value", heightBounds, initHeight, finalHeight); + problem.setStateInfo( + "/joint/height/speed", speedBounds, initSpeed, finalSpeed); + problem.setControlInfo("/forceset/muscle", + actuBounds, actuBounds.getLower()); + if (!ignoreActivationDynamics) { + problem.setStateInfo("/forceset/muscle/activation", + actuBounds, actuBounds.getLower()); + } + if (!ignoreTendonCompliance) { + problem.setStateInfo("/forceset/muscle/normalized_tendon_force", + normTendonBounds, normTendonBounds.getLower()); + } + + auto* tracking = problem.addGoal("tracking"); + + auto states = solutionTrajOpt.exportToStatesTable(); + TimeSeriesTable ref(states.getIndependentColumn()); + ref.addTableMetaData("inDegrees", std::string("no")); + ref.appendColumn("/joint/height/value", + states.getDependentColumn("/joint/height/value")); + // Tracking speed has a huge effect on getting a good solution for the + // control signal. + ref.appendColumn("/joint/height/speed", + states.getDependentColumn("/joint/height/speed")); + // Tracking joint/height/speed slightly increases the + // iterations to converge, and tracking activation cuts the iterations + // in half. + tracking->setReference(ref); + tracking->setAllowUnusedReferences(true); + + // Need a low-weighted effort cost so the problem is well-conditioned. + problem.addGoal("effort", 1e-3); + + auto& solver = study.initSolver(); + solver.set_num_mesh_intervals(30); + + MocoSolution solutionTrack = study.solve(); + std::string solutionFilename = + "testDeGrooteFregly2016Muscle_track_solution"; + if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; + if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; + solutionFilename += ".sto"; + //solutionTrack.write(solutionFilename); + double error = solutionTrack.compareContinuousVariablesRMS( + solutionTrajOpt, {{"states", {}}, {"controls", {}}}); + CHECK(error < 0.01); + } } } diff --git a/OpenSim/Moco/Test/testMocoConstraints.cpp b/OpenSim/Moco/Test/testMocoConstraints.cpp index ce4bbaf062..bbfe7daf23 100644 --- a/OpenSim/Moco/Test/testMocoConstraints.cpp +++ b/OpenSim/Moco/Test/testMocoConstraints.cpp @@ -882,7 +882,7 @@ TEST_CASE("DoublePendulum tests, Posa2016 method - MocoCasADiSolver", std::string dynamics_mode = GENERATE(as{}, "explicit", "implicit"); std::string section = fmt::format( - "enforce derivatives: {}, dynamics_mode: {}", + "enforce derivatives: {}, dynamics mode: {}", enforce_constraint_derivatives, dynamics_mode); DYNAMIC_SECTION(section) { @@ -895,8 +895,11 @@ TEST_CASE("DoublePendulum tests, Posa2016 method - MocoCasADiSolver", enforce_constraint_derivatives, dynamics_mode, "Posa2016"); } SECTION("PointOnLine") { + // This test struggles to converge with the default number of mesh + // intervals. testDoublePendulumPointOnLine( - enforce_constraint_derivatives, dynamics_mode, "Posa2016"); + enforce_constraint_derivatives, dynamics_mode, "Posa2016", + "hermite-simpson", 50); } } } @@ -1439,103 +1442,110 @@ TEST_CASE("Goals use Moco-defined accelerations and multipliers", "[casadi]") { } TEST_CASE("Multipliers are correct", "[casadi]") { - SECTION("Body welded to ground") { - auto dynamics_mode = - GENERATE(as{}, "implicit", "explicit"); - auto kinematic_constraint_method = - GENERATE(as{}, "Posa2016", "Bordalba2023"); - - Model model; - const double mass = 1.3169; - auto* body = new Body("body", mass, SimTK::Vec3(0), SimTK::Inertia(1)); - model.addBody(body); - - auto* joint = new PlanarJoint("joint", model.getGround(), *body); - model.addJoint(joint); - - auto* constr = new PointConstraint(model.getGround(), Vec3(0), - *body, Vec3(0)); - model.addConstraint(constr); - model.finalizeConnections(); - - MocoStudy study; - auto& problem = study.updProblem(); - problem.setModelAsCopy(model); - - problem.setTimeBounds(0, 0.5); - - auto& solver = study.initCasADiSolver(); - solver.set_num_mesh_intervals(5); - solver.set_multibody_dynamics_mode(dynamics_mode); - solver.set_kinematic_constraint_method(kinematic_constraint_method); - solver.set_transcription_scheme("hermite-simpson"); - solver.set_enforce_constraint_derivatives(true); - - MocoSolution solution = study.solve(); - - // Constraints 0 through 2 are the locks for the 3 translational DOFs. - const auto FX = solution.getMultiplier("lambda_cid3_p0"); - SimTK::Vector zero(FX); - zero.setToZero(); - OpenSim_CHECK_MATRIX_TOL(FX, zero, 1e-5); - const auto FY = solution.getMultiplier("lambda_cid3_p1"); - SimTK::Vector g(zero.size(), model.get_gravity()[1]); - OpenSim_CHECK_MATRIX_TOL(FY, mass * g, 1e-5); - const auto FZ = solution.getMultiplier("lambda_cid3_p2"); - OpenSim_CHECK_MATRIX_TOL(FZ, zero, 1e-5); - } - - // This problem is a point mass constrained to the line 0 = x - y. - // constraint Jacobian G is [1, -1]. - // m xdd + G(0) * lambda = Fx -> m xdd + lambda = Fx - // m ydd + G(1) * lambda = Fy -> m ydd - lambda = Fy - // Since xdd = ydd, we have: - // lambda = 0.5 * (Fx - Fy). - // This test ensures that the multiplier has the correct value. - SECTION("Planar point mass with CoordinateCouplerConstraint") { - - auto dynamics_mode = - GENERATE(as{}, "implicit", "explicit"); - auto kinematic_constraint_method = - GENERATE(as{}, "Posa2016", "Bordalba2023"); - - Model model = ModelFactory::createPlanarPointMass(); - model.set_gravity(Vec3(0)); - CoordinateCouplerConstraint* constraint = - new CoordinateCouplerConstraint(); - Array names; - names.append("tx"); - constraint->setIndependentCoordinateNames(names); - constraint->setDependentCoordinateName("ty"); - LinearFunction func(1.0, 0.0); - constraint->setFunction(func); - model.addConstraint(constraint); - - model.finalizeConnections(); - - MocoStudy study; - auto& problem = study.updProblem(); - problem.setModelAsCopy(model); - - problem.setTimeBounds(0, 1); - problem.setStateInfo("/jointset/tx/tx/value", {-5, 5}, 0, 3); - problem.setStateInfo("/jointset/tx/tx/speed", {-5, 5}, 0, 0); - problem.setControlInfo("/forceset/force_x", 0.5); + auto dynamics_mode = + GENERATE(as{}, "implicit", "explicit"); + auto kinematic_constraint_method = + GENERATE(as{}, "Posa2016", "Bordalba2023"); + std::string section = fmt::format( + "method: {}, dynamics mode: {}", + kinematic_constraint_method, dynamics_mode); - problem.addGoal(); + DYNAMIC_SECTION(section) { + SECTION("Body welded to ground") { + Model model; + const double mass = 1.3169; + auto* body = new Body("body", mass, SimTK::Vec3(0), + SimTK::Inertia(1)); + model.addBody(body); + + auto* joint = new PlanarJoint("joint", model.getGround(), *body); + model.addJoint(joint); - auto& solver = study.initCasADiSolver(); - solver.set_num_mesh_intervals(10); - solver.set_multibody_dynamics_mode(dynamics_mode); - solver.set_transcription_scheme("hermite-simpson"); - solver.set_kinematic_constraint_method(kinematic_constraint_method); - solver.set_enforce_constraint_derivatives(true); - MocoSolution solution = study.solve(); - const auto Fx = solution.getControl("/forceset/force_x"); - const auto Fy = solution.getControl("/forceset/force_y"); - const auto lambda = solution.getMultiplier("lambda_cid2_p0"); + auto* constr = new PointConstraint(model.getGround(), Vec3(0), + *body, Vec3(0)); + model.addConstraint(constr); + model.finalizeConnections(); + + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(model); + + problem.setTimeBounds(0, 0.5); + + auto& solver = study.initCasADiSolver(); + solver.set_num_mesh_intervals(5); + solver.set_multibody_dynamics_mode(dynamics_mode); + solver.set_kinematic_constraint_method(kinematic_constraint_method); + solver.set_transcription_scheme("hermite-simpson"); + solver.set_enforce_constraint_derivatives(true); + + MocoSolution solution = study.solve(); + + // Constraints 0 through 2 are the locks for the 3 translational + // DOFs. + const auto FX = solution.getMultiplier("lambda_cid3_p0"); + SimTK::Vector zero(FX); + zero.setToZero(); + OpenSim_CHECK_MATRIX_TOL(FX, zero, 1e-5); + const auto FY = solution.getMultiplier("lambda_cid3_p1"); + SimTK::Vector g(zero.size(), model.get_gravity()[1]); + OpenSim_CHECK_MATRIX_TOL(FY, mass * g, 1e-5); + const auto FZ = solution.getMultiplier("lambda_cid3_p2"); + OpenSim_CHECK_MATRIX_TOL(FZ, zero, 1e-5); + } - OpenSim_CHECK_MATRIX_TOL(lambda, 0.5 * (Fx - Fy), 1e-5); + // This problem is a point mass constrained to the line 0 = x - y. + // constraint Jacobian G is [1, -1]. + // m xdd + G(0) * lambda = Fx -> m xdd + lambda = Fx + // m ydd + G(1) * lambda = Fy -> m ydd - lambda = Fy + // Since xdd = ydd, we have: + // lambda = 0.5 * (Fx - Fy). + // This test ensures that the multiplier has the correct value. + SECTION("Planar point mass with CoordinateCouplerConstraint") { + + auto dynamics_mode = + GENERATE(as{}, "implicit", "explicit"); + auto kinematic_constraint_method = + GENERATE(as{}, "Posa2016", "Bordalba2023"); + + Model model = ModelFactory::createPlanarPointMass(); + model.set_gravity(Vec3(0)); + CoordinateCouplerConstraint* constraint = + new CoordinateCouplerConstraint(); + Array names; + names.append("tx"); + constraint->setIndependentCoordinateNames(names); + constraint->setDependentCoordinateName("ty"); + LinearFunction func(1.0, 0.0); + constraint->setFunction(func); + model.addConstraint(constraint); + + model.finalizeConnections(); + + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(model); + + problem.setTimeBounds(0, 1); + problem.setStateInfo("/jointset/tx/tx/value", {-5, 5}, 0, 3); + problem.setStateInfo("/jointset/tx/tx/speed", {-5, 5}, 0, 0); + problem.setControlInfo("/forceset/force_x", 0.5); + + problem.addGoal(); + + auto& solver = study.initCasADiSolver(); + solver.set_num_mesh_intervals(10); + solver.set_multibody_dynamics_mode(dynamics_mode); + solver.set_transcription_scheme("hermite-simpson"); + solver.set_kinematic_constraint_method(kinematic_constraint_method); + solver.set_enforce_constraint_derivatives(true); + MocoSolution solution = study.solve(); + const auto Fx = solution.getControl("/forceset/force_x"); + const auto Fy = solution.getControl("/forceset/force_y"); + const auto lambda = solution.getMultiplier("lambda_cid2_p0"); + + OpenSim_CHECK_MATRIX_TOL(lambda, 0.5 * (Fx - Fy), 1e-5); + } } } diff --git a/OpenSim/Moco/Test/testMocoInverse.cpp b/OpenSim/Moco/Test/testMocoInverse.cpp index 375d674dd6..b8e5a18152 100644 --- a/OpenSim/Moco/Test/testMocoInverse.cpp +++ b/OpenSim/Moco/Test/testMocoInverse.cpp @@ -142,15 +142,15 @@ TEST_CASE("MocoInverse Rajagopal2016, 18 muscles", "[casadi]") { inverse.set_final_time(1.0); inverse.set_kinematics_allow_extra_columns(true); inverse.set_mesh_interval(0.025); - inverse.set_constraint_tolerance(1e-4); - inverse.set_convergence_tolerance(1e-4); + inverse.set_constraint_tolerance(1e-5); + inverse.set_convergence_tolerance(1e-5); inverse.set_output_paths(0, ".*tendon_force.*"); inverse.set_output_paths(1, ".*fiber_force_along_tendon.*"); SECTION("Base problem") { MocoInverseSolution inverseSolution = inverse.solve(); MocoSolution solution = inverseSolution.getMocoSolution(); - //solution.write("testMocoInverse_subject_18musc_solution.sto"); + solution.write("testMocoInverse_subject_18musc_solution.sto"); MocoTrajectory std("std_testMocoInverse_subject_18musc_solution.sto"); const auto expected = std.getControlsTrajectory(); From a940c2c66904cf977c6258757239067baaf26cce Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 11 Sep 2024 18:42:21 -0700 Subject: [PATCH 31/38] Fixing more tests --- .../MocoCasADiSolver/CasOCTranscription.cpp | 50 +++++++++++++-- .../MocoCasADiSolver/CasOCTranscription.h | 64 ++++++------------- OpenSim/Moco/Test/testMocoActuators.cpp | 2 +- OpenSim/Moco/Test/testMocoGoals.cpp | 17 ++++- 4 files changed, 77 insertions(+), 56 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index df2a1d8a0f..8c5f42744a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -144,6 +144,13 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, const auto& info = m_problem.getEndpointConstraintInfos()[iec]; m_numConstraints += info.num_outputs; } + // Initial and final controls. + m_numInitialControlConstraints = + (1 - controlIndicesMap(0).scalar()) * m_problem.getNumControls(); + m_numConstraints += m_numInitialControlConstraints; + m_numFinalControlConstraints = + (1 - controlIndicesMap(-1).scalar()) * m_problem.getNumControls(); + m_numConstraints += m_numFinalControlConstraints; // Path constraints. m_constraints.path.resize(m_problem.getPathConstraintInfos().size()); m_numPathConstraintPoints = m_numControlPoints; @@ -188,9 +195,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, makeTimeIndices(meshInteriorIndicesVector); m_controlIndices = makeTimeIndices(controlIndicesVector); m_pathConstraintIndices = m_controlIndices; - // m_solver.getEnforcePathConstraintMeshInteriorPoints() - // ? makeTimeIndices(gridIndicesVector) - // : makeTimeIndices(meshIndicesVector); m_udotErrIndices = m_problem.isKinematicConstraintMethodBordalba2023() ? m_controlIndices : m_meshIndices; @@ -341,7 +345,6 @@ void Transcription::createVariablesAndSetBounds(const casadi::DM& grid, } } { - // TODO update this to set bounds properly based on the control points. const auto& controlInfos = m_problem.getControlInfos(); int ic = 0; for (const auto& info : controlInfos) { @@ -806,12 +809,12 @@ void Transcription::setObjectiveAndEndpointConstraints() { MXVector costOut; info.endpoint_function->call( - {m_unscaledVars[initial_time](0), + {m_unscaledVars[initial_time](-1), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), m_unscaledVars[derivatives](Slice(), 0), - m_unscaledVars[final_time](0), + m_unscaledVars[final_time](-1), m_unscaledVars[states](Slice(), -1), m_unscaledVars[controls](Slice(), -1), m_unscaledVars[multipliers](Slice(), -1), @@ -910,6 +913,41 @@ void Transcription::setObjectiveAndEndpointConstraints() { m_constraintsLowerBounds.endpoint[iec] = info.lowerBounds; m_constraintsUpperBounds.endpoint[iec] = info.upperBounds; } + + // Initial and final control constraints + // ------------------------------------- + m_constraints.initial_controls = MX(casadi::Sparsity::dense( + m_numInitialControlConstraints, 1)); + m_constraintsLowerBounds.initial_controls = + DM::zeros(m_numInitialControlConstraints, 1); + m_constraintsUpperBounds.initial_controls = + DM::zeros(m_numInitialControlConstraints, 1); + + m_constraints.final_controls = MX(casadi::Sparsity::dense( + m_numFinalControlConstraints, 1)); + m_constraintsLowerBounds.final_controls = + DM::zeros(m_numFinalControlConstraints, 1); + m_constraintsUpperBounds.final_controls = + DM::zeros(m_numFinalControlConstraints, 1); + + const auto& controlInfos = m_problem.getControlInfos(); + int ic = 0; + for (const auto& info : controlInfos) { + if (m_numInitialControlConstraints) { + m_constraints.initial_controls(ic) = m_unscaledVars[controls](ic, 0); + m_constraintsLowerBounds.initial_controls(ic) = + info.initialBounds.lower; + m_constraintsUpperBounds.initial_controls(ic) = + info.initialBounds.upper; + } + if (m_numFinalControlConstraints) { + m_constraints.final_controls(ic) = m_unscaledVars[controls](ic, -1); + m_constraintsLowerBounds.final_controls(ic) = info.finalBounds.lower; + m_constraintsUpperBounds.final_controls(ic) = info.finalBounds.upper; + } + ++ic; + } + } Solution Transcription::solve(const Iterate& guessOrig) { diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 67defd8cf0..093072c858 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -149,6 +149,8 @@ class Transcription { std::vector endpoint; std::vector path; T projection; + T initial_controls; + T final_controls; }; template @@ -180,6 +182,8 @@ class Transcription { int m_numConstraints = -1; int m_numPathConstraintPoints = -1; int m_numProjectionStates = -1; + int m_numInitialControlConstraints = -1; + int m_numFinalControlConstraints = -1; casadi::DM m_grid; casadi::MX m_times; casadi::MX m_parameters; @@ -505,20 +509,11 @@ class Transcription { } } - // Path constraints. - // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - // for (int i = 0; i < N; ++i) { - // for (const auto& path : constraints.path) { - // copyColumn(path, igrid + i); - // ng += path.rows(); - // } - // } - // } else { - // for (const auto& path : constraints.path) { - // copyColumn(path, imesh); - // ng += path.rows(); - // } - // } + // Initial controls. + if (imesh == 0) { + copyColumn(constraints.initial_controls, 0); + ng += constraints.initial_controls.rows(); + } // Projection constraints. if (imesh > 0) { @@ -551,17 +546,8 @@ class Transcription { ng += path.rows(); } } - // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - // for (const auto& path : constraints.path) { - // copyColumn(path, m_numGridPoints - 1); - // ng += path.rows(); - // } - // } else { - // for (const auto& path : constraints.path) { - // copyColumn(path, m_numMeshPoints - 1); - // ng += path.rows(); - // } - // } + copyColumn(constraints.final_controls, 0); + ng += constraints.final_controls.rows(); copyColumn(constraints.projection, m_numMeshIntervals - 1); ng += constraints.projection.rows(); flat.ng.push_back(ng); @@ -606,6 +592,8 @@ class Transcription { const auto& info = m_problem.getPathConstraintInfos()[ipc]; out.path[ipc] = init(info.size(), m_numPathConstraintPoints); } + out.initial_controls = init(m_numInitialControlConstraints, 1); + out.final_controls = init(m_numFinalControlConstraints, 1); int iflat = 0; auto copyColumn = [&flat, &iflat](T& matrix, int columnIndex) { @@ -658,18 +646,10 @@ class Transcription { } } - // Path constraints. - // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - // for (int i = 0; i < N; ++i) { - // for (auto& path : out.path) { - // copyColumn(path, igrid + i); - // } - // } - // } else { - // for (auto& path : out.path) { - // copyColumn(path, imesh); - // } - // } + // Initial controls. + if (imesh == 0) { + copyColumn(out.initial_controls, 0); + } // Projection constraints. if (imesh > 0) { @@ -692,15 +672,7 @@ class Transcription { copyColumn(path, idyn); } } - // if (m_solver.getEnforcePathConstraintMeshInteriorPoints()) { - // for (auto& path : out.path) { - // copyColumn(path, m_numGridPoints - 1); - // } - // } else { - // for (auto& path : out.path) { - // copyColumn(path, m_numMeshPoints - 1); - // } - // } + copyColumn(out.final_controls, 0); copyColumn(out.projection, m_numMeshIntervals - 1); OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index 7ce251be57..973e29e279 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -143,7 +143,7 @@ TEMPLATE_TEST_CASE( problem.addGoal("effort"); auto& solver = study.initSolver(); - solver.set_num_mesh_intervals(30); + solver.set_num_mesh_intervals(50); solver.set_multibody_dynamics_mode("explicit"); solver.set_optim_convergence_tolerance(1e-4); solver.set_optim_constraint_tolerance(1e-4); diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index bbcda978ca..bf4121d0fc 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -921,7 +921,7 @@ auto createStudy = []( problem.setStateInfo("/slider/position/value", {0, 1}, 0, 1); problem.setStateInfo("/slider/position/speed", {-100, 100}, initialSpeed, finalSpeed); - problem.setControlInfo("/actuator", MocoBounds(-10, 10)); + problem.setControlInfo("/actuator", MocoBounds(-100, 100)); return study; }; @@ -1292,26 +1292,37 @@ TEMPLATE_TEST_CASE("MocoOutputTrackingGoal", "", MocoCasADiSolver, Sine trackingFunction(0.5, SimTK::Pi / 2.0, 0.0, 0.0); auto studyTracking = createStudy({-100.0, 100.0}, {-100.0, 100.0}); auto& problemTracking = studyTracking.updProblem(); + problemTracking.setTimeBounds(0, 1); problemTracking.setStateInfo("/slider/position/value", {-5.0, 5.0}); problemTracking.template addGoal("effort", 1e-3); auto* goalTracking = problemTracking.template addGoal(); - goalTracking->setName("speed_tracking"); + goalTracking->setName("position_tracking"); goalTracking->setOutputPath("/body|position"); goalTracking->setExponent(2); goalTracking->setOutputIndex(0); goalTracking->setTrackingFunction(trackingFunction); auto& solverTracking = studyTracking.initSolver(); - solverTracking.set_num_mesh_intervals(10); + solverTracking.resetProblem(problemTracking); + solverTracking.set_num_mesh_intervals(25); MocoSolution solutionTracking = studyTracking.solve(); + solutionTracking.write("testMocoGoals_MocoOutputTrackingGoal_solution.sto"); auto solutionPosition = solutionTracking.getState("/slider/position/value"); SimTK::Vector time = solutionTracking.getTime(); + std::vector timeVec; + for (int itime = 0; itime < solutionTracking.getNumTimes(); ++itime) { + std::cout << "time: " << time[itime] << std::endl; + timeVec.push_back(time[itime]); + } SimTK::Vector trackedPosition(solutionTracking.getNumTimes(), 0.0); for (int itime = 0; itime < solutionTracking.getNumTimes(); ++itime) { SimTK::Vector timeVec(1, time[itime]); trackedPosition[itime] = trackingFunction.calcValue(timeVec); } + TimeSeriesTable trackedPositionTable(timeVec); + trackedPositionTable.appendColumn("/slider/position/value", trackedPosition); + STOFileAdapter::write(trackedPositionTable, "testMocoGoals_MocoOutputTrackingGoal_trackedPosition.sto"); auto trackingError = solutionPosition - trackedPosition; CHECK(trackingError.normRMS() == Approx(0).margin(1e-3)); } From b67ee60f8d1a82950bca1402d5b940253ed190b5 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 12 Sep 2024 14:36:54 -0700 Subject: [PATCH 32/38] Fixes for testMocoGoals, testMocoActuators, and testMocoImplicit --- OpenSim/Moco/Test/testMocoActuators.cpp | 479 +++++++++++------------- OpenSim/Moco/Test/testMocoGoals.cpp | 8 +- OpenSim/Moco/Test/testMocoImplicit.cpp | 81 ++-- 3 files changed, 271 insertions(+), 297 deletions(-) diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index 973e29e279..28de304027 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -76,30 +76,11 @@ namespace { return model; } -} - -TEMPLATE_TEST_CASE( - "Hanging muscle minimum time", "[casadi]", MocoCasADiSolver) { - // GENERATE creates separate tests in which these variables are set to - // either true or false. - auto ignoreActivationDynamics = GENERATE(true, false); - auto ignoreTendonCompliance = GENERATE(true, false); - auto isTendonDynamicsExplicit = GENERATE(true, false); - auto transcription_scheme = GENERATE(as{}, - "hermite-simpson", "legendre-gauss-3", "legendre-gauss-radau-3"); - std::string section = fmt::format( - "actdyn={}, tendyn={}, exptendyn={}, scheme={}", - !ignoreActivationDynamics, !ignoreTendonCompliance, - isTendonDynamicsExplicit, transcription_scheme); - DYNAMIC_SECTION(section) { - // CAPTURE prints the current value of these variables to the console. - CAPTURE(ignoreActivationDynamics); - CAPTURE(ignoreTendonCompliance); - CAPTURE(isTendonDynamicsExplicit); + MocoStudy createHangingMuscleStudy(const Model& model, + const std::string& scheme, bool ignoreActivationDynamics, + bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { - const double optimalFiberLength = 0.1; - const double tendonSlackLength = 0.05; SimTK::Real initHeight = 0.165; SimTK::Real finalHeight = 0.155; SimTK::Real initSpeed = 0; @@ -109,247 +90,243 @@ TEMPLATE_TEST_CASE( MocoBounds actuBounds(0.1, 1); MocoBounds normTendonBounds(0.1, 2); - Model model = createHangingMuscleModel(optimalFiberLength, - tendonSlackLength, ignoreActivationDynamics, ignoreTendonCompliance, - isTendonDynamicsExplicit); - - SimTK::State state = model.initSystem(); - - // Minimum time trajectory optimization. - // ------------------------------------- - const auto svn = model.getStateVariableNames(); - MocoSolution solutionTrajOpt; - std::string solutionFilename; - { - MocoStudy study; - MocoProblem& problem = study.updProblem(); - problem.setModelAsCopy(model); - problem.setTimeBounds(0, 0.5); - problem.setStateInfo( - "/joint/height/value", heightBounds, initHeight, finalHeight); - problem.setStateInfo( - "/joint/height/speed", speedBounds, initSpeed, finalSpeed); - problem.setControlInfo("/forceset/muscle", + MocoStudy study; + MocoProblem& problem = study.updProblem(); + problem.setModelAsCopy(model); + problem.setTimeBounds(0, 0.5); + problem.setStateInfo( + "/joint/height/value", heightBounds, initHeight, finalHeight); + problem.setStateInfo( + "/joint/height/speed", speedBounds, initSpeed, finalSpeed); + problem.setControlInfo("/forceset/muscle", + actuBounds, actuBounds.getLower()); + if (!ignoreActivationDynamics) { + problem.setStateInfo("/forceset/muscle/activation", actuBounds, actuBounds.getLower()); - if (!ignoreActivationDynamics) { - problem.setStateInfo("/forceset/muscle/activation", - actuBounds, actuBounds.getLower()); - } - if (!ignoreTendonCompliance) { - problem.setStateInfo("/forceset/muscle/normalized_tendon_force", - normTendonBounds, normTendonBounds.getLower()); - } - - problem.addGoal("effort"); - - auto& solver = study.initSolver(); - solver.set_num_mesh_intervals(50); - solver.set_multibody_dynamics_mode("explicit"); - solver.set_optim_convergence_tolerance(1e-4); - solver.set_optim_constraint_tolerance(1e-4); - solver.set_transcription_scheme(transcription_scheme); - - solutionTrajOpt = study.solve(); - solutionFilename = "testDeGrooteFregly2016Muscle_solution"; - if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; - if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; - - std::vector outputPaths; - outputPaths.emplace_back(".*tendon_force.*"); - outputPaths.emplace_back(".*fiber_force_along_tendon.*"); - outputPaths.emplace_back(".*length.*"); - outputPaths.emplace_back(".*velocity.*"); - auto table = study.analyze(solutionTrajOpt, outputPaths); - //STOFileAdapter::write(table, solutionFilename + "_outputs.sto"); - - solutionFilename += ".sto"; - solutionTrajOpt.write(solutionFilename); - - // Check that the muscle and tendon are in equilibrium. - const auto& muscle = model.getComponent( - "/forceset/muscle"); - auto statesTable = solutionTrajOpt.exportToStatesTable(); - const auto& activeFiberForceAlongTendon = table.getDependentColumn( - "/forceset/muscle|active_fiber_force_along_tendon"); - const auto& passiveFiberForceAlongTendon = table.getDependentColumn( - "/forceset/muscle|passive_fiber_force_along_tendon"); - const auto& tendonForce = table.getDependentColumn( - "/forceset/muscle|tendon_force"); - SimTK::Vector equilibriumResidual((int)table.getNumRows(), 0.0); - for (int i = 0; i < (int)table.getNumRows(); ++i) { - const double fiberForceAlongTendon = - activeFiberForceAlongTendon[i] + - passiveFiberForceAlongTendon[i]; - equilibriumResidual[i] = (tendonForce[i] - fiberForceAlongTendon) / - muscle.getMaxIsometricForce(); - } - CHECK(equilibriumResidual.normRMS() < 1e-3); } - - // Perform time stepping forward simulation using optimized controls. - // ------------------------------------------------------------------ - // See if we end up at the correct final state. - { - // We need to temporarily switch the muscle in the model to explicit - // tendon compliance dynamics mode to perform time stepping. - auto* mutableDGFMuscle = dynamic_cast( - &model.updComponent("forceset/muscle")); - if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { - mutableDGFMuscle->set_tendon_compliance_dynamics_mode("explicit"); - } - const auto trajSim = - simulateTrajectoryWithTimeStepping(solutionTrajOpt, model, 1e-6); - std::string trajFilename = "testDeGrooteFregly2016Muscle_timestepping"; - if (!ignoreActivationDynamics) trajFilename += "_actdyn"; - if (ignoreTendonCompliance) trajFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) trajFilename += "_exptendyn"; - trajFilename += ".sto"; - //trajSim.write(trajFilename); - - const double error = trajSim.compareContinuousVariablesRMS( - solutionTrajOpt, {{"states", {}}, {"controls", {}}}); - CHECK(error < 0.1); - if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { - mutableDGFMuscle->set_tendon_compliance_dynamics_mode("implicit"); - } + if (!ignoreTendonCompliance) { + problem.setStateInfo("/forceset/muscle/normalized_tendon_force", + normTendonBounds, normTendonBounds.getLower()); } - // Solve problem again using CMC. - // ------------------------------ - // See if we get the correct muscle activity. - if (!ignoreActivationDynamics) { - Model modelCMC = - createHangingMuscleModel(optimalFiberLength, tendonSlackLength, - ignoreActivationDynamics, ignoreTendonCompliance, true); - - // Need a reserve for CMC to solve. - auto* actu = new CoordinateActuator(); - actu->setName("actuator"); - actu->setOptimalForce(0.1); - actu->setMinControl(-100); - actu->setMaxControl(100); - actu->setCoordinate(&modelCMC.getCoordinateSet().get(0)); - modelCMC.addForce(actu); - modelCMC.finalizeConnections(); + problem.addGoal("effort"); - // Run CMC - // ------- - CMCTool cmc; - std::string cmcFilename = "testDeGrooteFregly2016Muscle_cmc"; - cmc.setResultsDir(cmcFilename); - if (!ignoreActivationDynamics) cmcFilename += "_actdyn"; - if (ignoreTendonCompliance) cmcFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) cmcFilename += "_exptendyn"; - cmc.setName(cmcFilename); - cmc.setModel(modelCMC); - cmc.setInitialTime(0); - const double finalTime = - solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; - cmc.setFinalTime(finalTime); - cmc.setDesiredKinematicsFileName(solutionFilename); - cmc.setTaskSetFileName("hanging_muscle_cmc_tasks.xml"); - cmc.setSolveForEquilibrium(false); - cmc.setTimeWindow(0.01); - cmc.setUseFastTarget(true); - cmc.run(); + auto& solver = study.initCasADiSolver(); + solver.set_num_mesh_intervals(50); + solver.set_multibody_dynamics_mode("explicit"); + solver.set_optim_convergence_tolerance(1e-6); + solver.set_optim_constraint_tolerance(1e-6); + solver.set_transcription_scheme(scheme); + solver.set_minimize_implicit_auxiliary_derivatives(true); + solver.set_implicit_auxiliary_derivatives_weight(1e-3); - // Create a MocoTrajectory from the CMC solution. - TimeSeriesTable cmcStates; - std::vector stateColumnLabels; - cmcStates = TimeSeriesTable("testDeGrooteFregly2016Muscle_cmc/" + - cmcFilename + "_states.sto"); - stateColumnLabels = cmcStates.getColumnLabels(); - - TimeSeriesTable cmcControls("testDeGrooteFregly2016Muscle_cmc/" + - cmcFilename + "_controls.sto"); - const auto& stdTime = cmcControls.getIndependentColumn(); - SimTK::Vector time((int)stdTime.size(), stdTime.data()); - std::vector controlNames{"/forceset/muscle"}; - MocoTrajectory cmcTraj(time, stateColumnLabels, controlNames, {}, {}, - cmcStates.getMatrix(), - cmcControls.getMatrixBlock(0, 0, cmcControls.getNumRows(), 1), - SimTK::Matrix(), SimTK::RowVector()); - //cmcTraj.write( - // "testDeGrooteFregly2016Muscle_cmc/" + cmcFilename + ".sto"); + return study; + } +} - // Compare CMC solution to the Moco solution. - std::vector states; - states.push_back("/forceset/muscle/activation"); - if (!ignoreTendonCompliance) { - states.push_back("/forceset/muscle/normalized_tendon_force"); - } - // Only compare states, since CMC controls are spiky. Only need a rough - // comparison here, just confirming that CMC works with the muscle model. - const double error = cmcTraj.compareContinuousVariablesRMS( - solutionTrajOpt, {{"states", states}}); - CHECK(error < 0.5); +TEST_CASE("Hanging muscle minimum time", "[casadi]") { + auto ignoreActivationDynamics = GENERATE(true, false); + auto ignoreTendonCompliance = GENERATE(true, false); + auto isTendonDynamicsExplicit = GENERATE(true, false); + auto transcription_scheme = GENERATE(as{}, + "hermite-simpson", "legendre-gauss-3", "legendre-gauss-radau-3"); + CAPTURE(ignoreActivationDynamics); + CAPTURE(ignoreTendonCompliance); + CAPTURE(isTendonDynamicsExplicit); + + // Create the hanging muscle model. + const double optimalFiberLength = 0.1; + const double tendonSlackLength = 0.05; + Model model = createHangingMuscleModel(optimalFiberLength, + tendonSlackLength, ignoreActivationDynamics, + ignoreTendonCompliance, isTendonDynamicsExplicit); + model.initSystem(); + + // Minimum effort trajectory optimization. + // --------------------------------------- + MocoSolution solutionTrajOpt; + std::string solutionFilename; + { + MocoStudy study = createHangingMuscleStudy(model, + transcription_scheme, ignoreActivationDynamics, + ignoreTendonCompliance, isTendonDynamicsExplicit); + solutionTrajOpt = study.solve(); + solutionFilename = "testDeGrooteFregly2016Muscle_solution"; + if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; + if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; + + std::vector outputPaths; + outputPaths.emplace_back(".*tendon_force.*"); + outputPaths.emplace_back(".*fiber_force_along_tendon.*"); + outputPaths.emplace_back(".*length.*"); + outputPaths.emplace_back(".*velocity.*"); + auto table = study.analyze(solutionTrajOpt, outputPaths); + STOFileAdapter::write(table, solutionFilename + "_outputs.sto"); + + solutionFilename += ".sto"; + solutionTrajOpt.write(solutionFilename); + + // Check that the muscle and tendon are in equilibrium. + const auto& muscle = model.getComponent( + "/forceset/muscle"); + auto statesTable = solutionTrajOpt.exportToStatesTable(); + const auto& activeFiberForceAlongTendon = table.getDependentColumn( + "/forceset/muscle|active_fiber_force_along_tendon"); + const auto& passiveFiberForceAlongTendon = table.getDependentColumn( + "/forceset/muscle|passive_fiber_force_along_tendon"); + const auto& tendonForce = table.getDependentColumn( + "/forceset/muscle|tendon_force"); + SimTK::Vector equilibriumResidual((int)table.getNumRows(), 0.0); + for (int i = 0; i < (int)table.getNumRows(); ++i) { + const double fiberForceAlongTendon = + activeFiberForceAlongTendon[i] + + passiveFiberForceAlongTendon[i]; + equilibriumResidual[i] = (tendonForce[i] - fiberForceAlongTendon) / + muscle.getMaxIsometricForce(); } + CHECK(equilibriumResidual.normRMS() < 1e-3); + } - // Track the kinematics from the trajectory optimization. - // ------------------------------------------------------ - // We will try to recover muscle activity. - if (!isTendonDynamicsExplicit) { - std::cout << "Tracking the trajectory optimization coordinate solution." - << std::endl; - MocoStudy study; - MocoProblem& problem = study.updProblem(); - problem.setModelAsCopy(model); - // Using an equality constraint for the time bounds was essential for - // recovering the correct excitation. - const double finalTime = - solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; - problem.setTimeBounds(0, finalTime); - problem.setStateInfo( - "/joint/height/value", heightBounds, initHeight, finalHeight); - problem.setStateInfo( - "/joint/height/speed", speedBounds, initSpeed, finalSpeed); - problem.setControlInfo("/forceset/muscle", - actuBounds, actuBounds.getLower()); - if (!ignoreActivationDynamics) { - problem.setStateInfo("/forceset/muscle/activation", - actuBounds, actuBounds.getLower()); - } - if (!ignoreTendonCompliance) { - problem.setStateInfo("/forceset/muscle/normalized_tendon_force", - normTendonBounds, normTendonBounds.getLower()); - } - - auto* tracking = problem.addGoal("tracking"); - - auto states = solutionTrajOpt.exportToStatesTable(); - TimeSeriesTable ref(states.getIndependentColumn()); - ref.addTableMetaData("inDegrees", std::string("no")); - ref.appendColumn("/joint/height/value", - states.getDependentColumn("/joint/height/value")); - // Tracking speed has a huge effect on getting a good solution for the - // control signal. - ref.appendColumn("/joint/height/speed", - states.getDependentColumn("/joint/height/speed")); - // Tracking joint/height/speed slightly increases the - // iterations to converge, and tracking activation cuts the iterations - // in half. - tracking->setReference(ref); - tracking->setAllowUnusedReferences(true); - - // Need a low-weighted effort cost so the problem is well-conditioned. - problem.addGoal("effort", 1e-3); + // Perform time stepping forward simulation using optimized controls. + // ------------------------------------------------------------------ + // See if we end up at the correct final state. + { + // We need to temporarily switch the muscle in the model to explicit + // tendon compliance dynamics mode to perform time stepping. + auto* mutableDGFMuscle = dynamic_cast( + &model.updComponent("forceset/muscle")); + if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { + mutableDGFMuscle->set_tendon_compliance_dynamics_mode("explicit"); + } + const auto trajSim = + simulateTrajectoryWithTimeStepping(solutionTrajOpt, model, 1e-6); + std::string trajFilename = "testDeGrooteFregly2016Muscle_timestepping"; + if (!ignoreActivationDynamics) trajFilename += "_actdyn"; + if (ignoreTendonCompliance) trajFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) trajFilename += "_exptendyn"; + trajFilename += ".sto"; + //trajSim.write(trajFilename); + + const double error = trajSim.compareContinuousVariablesRMS( + solutionTrajOpt, {{"states", {}}, {"controls", {}}}); + CHECK(error < 0.1); + if (!ignoreTendonCompliance && !isTendonDynamicsExplicit) { + mutableDGFMuscle->set_tendon_compliance_dynamics_mode("implicit"); + } + } - auto& solver = study.initSolver(); - solver.set_num_mesh_intervals(30); + // Solve problem again using CMC. + // ------------------------------ + // See if we get the correct muscle activity. + if (!ignoreActivationDynamics) { + Model modelCMC = createHangingMuscleModel(optimalFiberLength, + tendonSlackLength, ignoreActivationDynamics, + ignoreTendonCompliance, true); + + // Need a reserve for CMC to solve. + auto* actu = new CoordinateActuator(); + actu->setName("actuator"); + actu->setOptimalForce(0.1); + actu->setMinControl(-100); + actu->setMaxControl(100); + actu->setCoordinate(&modelCMC.getCoordinateSet().get(0)); + modelCMC.addForce(actu); + modelCMC.finalizeConnections(); + + // Run CMC + // ------- + CMCTool cmc; + std::string cmcFilename = "testDeGrooteFregly2016Muscle_cmc"; + cmc.setResultsDir(cmcFilename); + if (!ignoreActivationDynamics) cmcFilename += "_actdyn"; + if (ignoreTendonCompliance) cmcFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) cmcFilename += "_exptendyn"; + cmc.setName(cmcFilename); + cmc.setModel(modelCMC); + cmc.setInitialTime(0); + const double finalTime = + solutionTrajOpt.getTime()[solutionTrajOpt.getNumTimes() - 1]; + cmc.setFinalTime(finalTime); + cmc.setDesiredKinematicsFileName(solutionFilename); + cmc.setTaskSetFileName("hanging_muscle_cmc_tasks.xml"); + cmc.setSolveForEquilibrium(false); + cmc.setTimeWindow(0.01); + cmc.setUseFastTarget(true); + cmc.run(); + + // Create a MocoTrajectory from the CMC solution. + TimeSeriesTable cmcStates; + std::vector stateColumnLabels; + cmcStates = TimeSeriesTable("testDeGrooteFregly2016Muscle_cmc/" + + cmcFilename + "_states.sto"); + stateColumnLabels = cmcStates.getColumnLabels(); + + TimeSeriesTable cmcControls("testDeGrooteFregly2016Muscle_cmc/" + + cmcFilename + "_controls.sto"); + const auto& stdTime = cmcControls.getIndependentColumn(); + SimTK::Vector time((int)stdTime.size(), stdTime.data()); + std::vector controlNames{"/forceset/muscle"}; + MocoTrajectory cmcTraj(time, stateColumnLabels, controlNames, {}, {}, + cmcStates.getMatrix(), + cmcControls.getMatrixBlock(0, 0, cmcControls.getNumRows(), 1), + SimTK::Matrix(), SimTK::RowVector()); + //cmcTraj.write( + // "testDeGrooteFregly2016Muscle_cmc/" + cmcFilename + ".sto"); + + // Compare CMC solution to the Moco solution. + std::vector states; + states.push_back("/forceset/muscle/activation"); + if (!ignoreTendonCompliance) { + states.push_back("/forceset/muscle/normalized_tendon_force"); + } + // Only compare states, since CMC controls are spiky. Only need a rough + // comparison here, just confirming that CMC works with the muscle model. + const double error = cmcTraj.compareContinuousVariablesRMS( + solutionTrajOpt, {{"states", states}}); + CHECK(error < 0.5); + } - MocoSolution solutionTrack = study.solve(); - std::string solutionFilename = - "testDeGrooteFregly2016Muscle_track_solution"; - if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; - if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; - if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; - solutionFilename += ".sto"; - //solutionTrack.write(solutionFilename); - double error = solutionTrack.compareContinuousVariablesRMS( - solutionTrajOpt, {{"states", {}}, {"controls", {}}}); - CHECK(error < 0.01); + // Track the kinematics from the trajectory optimization. + // ------------------------------------------------------ + // Check that we get the same states and controls with a tracking goal + // added to the problem. + if (!isTendonDynamicsExplicit) { + MocoStudy study = createHangingMuscleStudy(model, + transcription_scheme, ignoreActivationDynamics, + ignoreTendonCompliance, isTendonDynamicsExplicit); + + // Add a state tracking goal to the original problem. + auto& problem = study.updProblem(); + auto* tracking = problem.addGoal("tracking"); + tracking->setWeight(100.0); + auto states = solutionTrajOpt.exportToStatesTable(); + TimeSeriesTable ref(states.getIndependentColumn()); + ref.addTableMetaData("inDegrees", std::string("no")); + ref.appendColumn("/joint/height/value", + states.getDependentColumn("/joint/height/value")); + ref.appendColumn("/joint/height/speed", + states.getDependentColumn("/joint/height/speed")); + if (!ignoreTendonCompliance) { + ref.appendColumn("/forceset/muscle/normalized_tendon_force", + states.getDependentColumn( + "/forceset/muscle/normalized_tendon_force")); } + tracking->setReference(ref); + auto& solver = study.updSolver(); + solver.resetProblem(problem); + + MocoSolution solutionTrack = study.solve(); + std::string solutionFilename = + "testDeGrooteFregly2016Muscle_track_solution"; + if (!ignoreActivationDynamics) solutionFilename += "_actdyn"; + if (ignoreTendonCompliance) solutionFilename += "_rigidtendon"; + if (isTendonDynamicsExplicit) solutionFilename += "_exptendyn"; + solutionFilename += ".sto"; + solutionTrack.write(solutionFilename); + double error = solutionTrack.compareContinuousVariablesRMS( + solutionTrajOpt, {{"states", {}}, {"controls", {}}}); + CHECK(error < 0.01); } } diff --git a/OpenSim/Moco/Test/testMocoGoals.cpp b/OpenSim/Moco/Test/testMocoGoals.cpp index bf4121d0fc..80662a6a54 100644 --- a/OpenSim/Moco/Test/testMocoGoals.cpp +++ b/OpenSim/Moco/Test/testMocoGoals.cpp @@ -933,15 +933,17 @@ TEMPLATE_TEST_CASE("MocoOutputGoal", "", MocoCasADiSolver, { auto study = createStudy(0, 0); auto& problem = study.updProblem(); + problem.setTimeBounds(0, 2); problem.template addGoal(); auto &solver = study.template initSolver(); - solver.set_num_mesh_intervals(10); + solver.set_num_mesh_intervals(20); solutionControl = study.solve(); } MocoSolution solutionOutput; { auto study = createStudy(0, 0); auto& problem = study.updProblem(); + problem.setTimeBounds(0, 2); auto model = createSlidingMassModel(); auto* component = new MySumSquaredControls(); @@ -953,7 +955,7 @@ TEMPLATE_TEST_CASE("MocoOutputGoal", "", MocoCasADiSolver, goal->setOutputPath("/mysumsquaredcontrols|sum_squared_controls"); auto& solver = study.template initSolver(); - solver.set_num_mesh_intervals(10); + solver.set_num_mesh_intervals(20); solutionOutput = study.solve(); } @@ -1289,7 +1291,7 @@ TEMPLATE_TEST_CASE("MocoOutputTrackingGoal", "", MocoCasADiSolver, // Have a sliding mass track a sinusoidal function. // ------------------------------------------------ - Sine trackingFunction(0.5, SimTK::Pi / 2.0, 0.0, 0.0); + LinearFunction trackingFunction(1.0, -1.0); auto studyTracking = createStudy({-100.0, 100.0}, {-100.0, 100.0}); auto& problemTracking = studyTracking.updProblem(); problemTracking.setTimeBounds(0, 1); diff --git a/OpenSim/Moco/Test/testMocoImplicit.cpp b/OpenSim/Moco/Test/testMocoImplicit.cpp index 549cf1911c..1ee3da9ff0 100644 --- a/OpenSim/Moco/Test/testMocoImplicit.cpp +++ b/OpenSim/Moco/Test/testMocoImplicit.cpp @@ -77,19 +77,23 @@ MocoSolution solveDoublePendulumSwingup(const std::string& dynamics_mode) { ftCost->setWeight(0.001); // Make sure the end-effector reaches the final target. auto* finalCost = mp.addGoal("final"); - finalCost->setWeight(1000.0); + finalCost->setWeight(100.0); finalCost->setPointName("/markerset/marker1"); finalCost->setReferenceLocation(SimTK::Vec3(0, 2, 0)); + // This effort term helps provide consistent solutions between implicit and + // explicit dynamics and avoid local minima where the time range converges + // towards [0, 0]. + auto* effort = mp.addGoal("effort"); // Configure the solver. // ===================== - int N = 29; // 29 mesh intervals = 30 mesh points + int N = 25; auto& solver = study.initSolver(); solver.set_multibody_dynamics_mode(dynamics_mode); solver.set_num_mesh_intervals(N); solver.set_transcription_scheme("trapezoidal"); - // solver.set_verbosity(2); + // Set an initial guess. MocoTrajectory guess = solver.createGuess(); guess.resampleWithNumTimes(2); guess.setTime({0, 1}); @@ -102,70 +106,61 @@ MocoSolution solveDoublePendulumSwingup(const std::string& dynamics_mode) { guess.resampleWithNumTimes(10); solver.setGuess(guess); - // moco.visualize(guess); - - study.print("double_pendulum_swingup_" + dynamics_mode + ".omoco"); - // Solve the problem. // ================== MocoSolution solution = study.solve(); + solution.write(fmt::format( + "testMocoImplicit_testDoublePendulumSwingup_{}.sto", dynamics_mode)); // study.visualize(solution); return solution; } -// TODO does not pass consistently on Mac -//TEMPLATE_TEST_CASE("Two consecutive problems produce the same solution", "", -// MocoCasADiSolver /*, MocoTropterSolver*/) { -// auto dynamics_mode = GENERATE(as{}, "implicit", "explicit"); -// -// auto solution1 = solveDoublePendulumSwingup(dynamics_mode); -// auto solution2 = solveDoublePendulumSwingup(dynamics_mode); -// -// const double stateError = solution1.compareContinuousVariablesRMS( -// solution2, {{"states", {}}}); -// -// const double controlError = solution1.compareContinuousVariablesRMS( -// solution2, {{"controls", {}}}); -// -// CAPTURE(stateError, controlError); -// -// // Solutions are approximately equal. -// CHECK(solution1.getFinalTime() == -// Approx(solution2.getFinalTime()).margin(1e-2)); -// CHECK(stateError == Approx(0)); -// CHECK(controlError == Approx(0)); -//} - -// TODO not passing consistently for MocoTropterSolver on Mac +// TODO does not pass with MocoTropterSolver. +TEMPLATE_TEST_CASE("Two consecutive problems produce the same solution", "", + MocoCasADiSolver /**, MocoTropterSolver*/) { + auto dynamics_mode = GENERATE(as{}, "implicit", "explicit"); + DYNAMIC_SECTION(fmt::format("dynamics mode: {}", dynamics_mode)) { + auto solution1 = solveDoublePendulumSwingup(dynamics_mode); + auto solution2 = solveDoublePendulumSwingup(dynamics_mode); + + const double stateError = solution1.compareContinuousVariablesRMS( + solution2, {{"states", {}}}); + const double controlError = solution1.compareContinuousVariablesRMS( + solution2, {{"controls", {}}}); + + CAPTURE(stateError, controlError); + + // Solutions are approximately equal. + CHECK(solution1.getFinalTime() == + Approx(solution2.getFinalTime()).margin(1e-2)); + CHECK(stateError == Approx(0)); + CHECK(controlError == Approx(0)); + } +} + TEMPLATE_TEST_CASE("Similar solutions between implicit and explicit dynamics", - "[linux/win][implicit]", MocoCasADiSolver /*,MocoTropterSolver*/) { + "[implicit]", MocoCasADiSolver, MocoTropterSolver) { GIVEN("solutions to implicit and explicit problems") { - auto solutionImplicit = - solveDoublePendulumSwingup("implicit"); - // TODO: The solution to this explicit problem changes every time. + auto solutionImplicit = solveDoublePendulumSwingup("implicit"); auto solution = solveDoublePendulumSwingup("explicit"); - CAPTURE(solutionImplicit.getFinalTime(), solution.getFinalTime()); const double stateError = solutionImplicit.compareContinuousVariablesRMS( solution, {{"states", {}}}); - - // There is more deviation in the controls. const double controlError = solutionImplicit.compareContinuousVariablesRMS( solution, {{"controls", {}}}); - CAPTURE(stateError, controlError); // Solutions are approximately equal. CHECK(solutionImplicit.getFinalTime() == - Approx(solution.getFinalTime()).margin(1e-2)); - CHECK(stateError < 2.0); - CHECK(controlError < 30.0); + Approx(solution.getFinalTime()).margin(1e-3)); + CHECK(stateError < 1.0); + CHECK(controlError < 1.0); // Accelerations are correct. auto table = solution.exportToStatesTable(); @@ -186,7 +181,7 @@ TEMPLATE_TEST_CASE("Similar solutions between implicit and explicit dynamics", const double RMS = solutionImplicit.compareContinuousVariablesRMS( explicitWithDeriv, {{"derivatives", {}}}); CAPTURE(RMS); - CHECK(RMS < 35.0); + CHECK(RMS < 1.0); } } From d3d86d40140598a8ed5856334bb874530b7179e8 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 12 Sep 2024 14:37:24 -0700 Subject: [PATCH 33/38] Use initial and final indices for endpoint function calls in CasOCTranscription --- OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 8c5f42744a..027464be5d 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -809,7 +809,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { MXVector costOut; info.endpoint_function->call( - {m_unscaledVars[initial_time](-1), + {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), @@ -896,7 +896,7 @@ void Transcription::setObjectiveAndEndpointConstraints() { MXVector endpointOut; info.endpoint_function->call( - {m_unscaledVars[initial_time](-1), + {m_unscaledVars[initial_time](0), m_unscaledVars[states](Slice(), 0), m_unscaledVars[controls](Slice(), 0), m_unscaledVars[multipliers](Slice(), 0), From 66411e1abc098cc5bb016e7439a4d0b19b03e9d3 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 13 Sep 2024 14:05:54 -0700 Subject: [PATCH 34/38] Fix last set of failing tests --- ..._testMocoTrackGait10dof18musc_solution.sto | 115 +++++++++--------- OpenSim/Moco/Test/testMocoConstraints.cpp | 6 +- OpenSim/Moco/Test/testMocoTrack.cpp | 43 +++++-- 3 files changed, 94 insertions(+), 70 deletions(-) diff --git a/OpenSim/Moco/Test/std_testMocoTrackGait10dof18musc_solution.sto b/OpenSim/Moco/Test/std_testMocoTrackGait10dof18musc_solution.sto index 818ffe970d..783f755441 100644 --- a/OpenSim/Moco/Test/std_testMocoTrackGait10dof18musc_solution.sto +++ b/OpenSim/Moco/Test/std_testMocoTrackGait10dof18musc_solution.sto @@ -1,70 +1,71 @@ inDegrees=no num_controls=10 num_derivatives=0 -num_iterations=96 +num_input_controls=0 +num_iterations=369 num_multipliers=0 num_parameters=0 num_slacks=0 num_states=20 -objective=0.039827 -objective_control_effort=0.000569 -objective_state_tracking=0.039258 -solver_duration=14.912321 +objective=0.000460 +objective_control_effort=0.000454 +objective_state_tracking=0.000006 +solver_duration=22.778436 status=Solve_Succeeded success=true DataType=double version=3 -OpenSimVersion=4.5-2024-05-23-2aaa17cc3 +OpenSimVersion=4.5.1-2024-09-10-8f74b2ae2 endheader time /jointset/ground_pelvis/pelvis_tilt/value /jointset/ground_pelvis/pelvis_tx/value /jointset/ground_pelvis/pelvis_ty/value /jointset/hip_r/hip_flexion_r/value /jointset/hip_l/hip_flexion_l/value /jointset/back/lumbar_extension/value /jointset/knee_r/knee_angle_r/value /jointset/knee_l/knee_angle_l/value /jointset/ankle_r/ankle_angle_r/value /jointset/ankle_l/ankle_angle_l/value /jointset/ground_pelvis/pelvis_tilt/speed /jointset/ground_pelvis/pelvis_tx/speed /jointset/ground_pelvis/pelvis_ty/speed /jointset/hip_r/hip_flexion_r/speed /jointset/hip_l/hip_flexion_l/speed /jointset/back/lumbar_extension/speed /jointset/knee_r/knee_angle_r/speed /jointset/knee_l/knee_angle_l/speed /jointset/ankle_r/ankle_angle_r/speed /jointset/ankle_l/ankle_angle_l/speed /forceset/reserve_jointset_ground_pelvis_pelvis_tilt /forceset/reserve_jointset_ground_pelvis_pelvis_tx /forceset/reserve_jointset_ground_pelvis_pelvis_ty /forceset/reserve_jointset_hip_r_hip_flexion_r /forceset/reserve_jointset_knee_r_knee_angle_r /forceset/reserve_jointset_ankle_r_ankle_angle_r /forceset/reserve_jointset_hip_l_hip_flexion_l /forceset/reserve_jointset_knee_l_knee_angle_l /forceset/reserve_jointset_ankle_l_ankle_angle_l /forceset/reserve_jointset_back_lumbar_extension -0.5 0.1238566857614891 0.595728675821568 1.030743958342165 0.2814677473562928 -0.3102105808216573 -0.4051407504382534 -0.3097699984784035 0.02256476267432119 0.01142064493933395 0.1006589233709155 0.1101424119148011 -0.05319330152205409 -0.1845313117807712 -1.151820431963396 -1.795284945516493 -0.1147223502555851 7.364634089216715 0.8841021281107317 -0.78317866777154 0.4702021040107962 0.5357164490271628 0.1530172689621669 0.02758232575436812 0.017897431432023 -0.0742824077186533 0.01839715715306948 0.3444822860015452 -0.3137155535151984 -0.6958204746507889 0.2963405525232248 -0.51 0.1251021990788648 0.5952542125426662 1.028840229324745 0.2698466126884242 -0.327981357748978 -0.4062738683794063 -0.2376855912215922 0.03118365691168998 0.003995670291067012 0.1055070639338989 0.1342928146799538 -0.04225642087273972 -0.1961897771215814 -1.162749997958942 -1.762944531005356 -0.1122178866158478 7.025754975985474 0.8318564026089247 -0.6965694825592856 0.4973367265704923 0.006359803348691015 -0.161980751309028 0.03612202789288539 -0.05420988741715211 -0.1014025268719495 0.01485021162099673 0.1012233286963142 -0.4153854055720829 -0.7175515579969809 0.1514977309118984 -0.52 0.1264491933174805 0.5948724060718243 1.026820657091369 0.2584058774701415 -0.3455509532629186 -0.4073914411433875 -0.1697847466818952 0.0390454843914177 -0.002405809125400545 0.1105638722620177 0.1304385961627969 -0.03466193991007748 -0.2076999549718079 -1.115740542046219 -1.755048662840479 -0.1116133148212511 6.527921545793841 0.7326887765825194 -0.5784796214116638 0.511935657037878 -0.5229968423297807 -0.4769787715802229 0.04466173003140266 -0.1263172062663272 -0.1285226460252458 0.01130326608892398 -0.1420356286089167 -0.5170552576289674 -0.739282641343173 0.00665490930057207 -0.53 0.1276954193734887 0.5945580405942085 1.024694753299142 0.2477345417027745 -0.3630288931520651 -0.4084982500504283 -0.1076645078364824 0.04575047568719238 -0.007472346327191685 0.1155252820006581 0.1226433238996934 -0.02784855064825366 -0.2166237077738101 -1.011159073649261 -1.731595459098222 -0.1092399190876268 5.872209579915321 0.6065053765208288 -0.4381888833313182 0.4988064932251451 0.02486237273137556 -0.1210134482659492 0.04126443420239145 -0.06338484167531702 -0.1211873375504933 0.007337725192757511 0.1390956057237788 -0.4400330924607013 -0.7520654226397901 0.1512771630349797 -0.54 0.1289787939726915 0.5943226871581558 1.02250532484989 0.2383300467527146 -0.380003985327071 -0.4095660685751547 -0.05281888795105655 0.05113950980080258 -0.01123680807972202 0.1209092061772196 0.1378683048017252 -0.01885953159743382 -0.2204048863765542 -0.8623723785848125 -1.654479120012355 -0.1038152383583992 5.072997753796455 0.4694973401496256 -0.3180645315595033 0.5964385446220989 0.5727215877925318 0.2349518750483246 0.03786713837338024 -0.0004524770843068126 -0.1138520290757408 0.003372184296591045 0.4202268400564744 -0.3630109272924352 -0.7648482039364071 0.2958994167693873 -0.55 0.1304866631884807 0.5941829094031633 1.020306236055845 0.2305694045799429 -0.396079731434516 -0.4106019416602247 -0.006581623540821079 0.05510314512828337 -0.01378934486553695 0.1267202789982396 0.1602109056545028 -0.009313222197639776 -0.2188143014244072 -0.6845537320278419 -1.56154110164401 -0.1032024569950914 4.158753738029565 0.3187740227861172 -0.1909015974011514 0.5522938790503574 0.1968194276179768 0.07138524714734042 0.1140498864043551 -0.08594637302160717 -0.1503939988179153 -0.0009608485854197455 0.2917600776694237 -0.4240151709832917 -0.7922690936198646 0.1941780343296745 -0.5600000000000001 0.132113119431749 0.594132078658272 1.018141010241562 0.2247430185909915 -0.4112522273632988 -0.4116269792818465 0.03004215900511349 0.05742587620531667 -0.01502401546369844 0.1316854409475958 0.1615857102975266 -0.001070129577180701 -0.2136322904242342 -0.4755211418207519 -1.473829084279929 -0.1016481456687696 3.150301380936275 0.1413164900601304 -0.05449129402881617 0.4272563702893362 -0.1790827325565783 -0.09218138075364381 0.19023263443533 -0.1714402689589075 -0.1869359685600898 -0.005293881467430536 0.1632933152823731 -0.4850194146741483 -0.8196899833033223 0.09245665188996167 -0.5700000000000001 0.1336531469113581 0.5941557391051608 1.016042095593187 0.2211371988774538 -0.4255302027847316 -0.412612804122216 0.05626460067503065 0.05784548819720856 -0.01510101687944891 0.1366464044393675 0.146129253144008 0.005566363494323022 -0.205520346461029 -0.2460219472947331 -1.382065025232359 -0.09636544162377707 2.093467596977925 -0.05882909558635367 0.01167442313093338 0.5461423257147248 -0.2737286918710448 -0.2384325568855017 0.2362103201971509 -0.1965431918422018 -0.1979460884138129 -0.009178581427326688 0.1347543633226216 -0.5072364385233095 -0.8471586903953481 0.07673684373781493 -0.58 0.1350298938450236 0.5942386888189461 1.014043209168135 0.2198149967169379 -0.4388995083724603 -0.4135712604986951 0.07189712382328668 0.05622059421549756 -0.01533885875603555 0.1422324074148843 0.1289296011088358 0.01078772380214795 -0.1936266457598182 -0.01879763121638241 -1.292095117539119 -0.09617445289069386 1.032317676604039 -0.2675847046604486 -0.0866593861960509 0.5522642670383412 -0.3683746511855112 -0.3846837330173596 0.2821880059589718 -0.2216461147254962 -0.2089562082675361 -0.01306328138722284 0.1062154113628702 -0.5294534623724705 -0.8746273974873738 0.06101703558566818 -0.59 0.1362688648701185 0.5943813807294366 1.012179151481743 0.2207191938968719 -0.4513450793238342 -0.4145274967120564 0.07699319587729501 0.05249033825104369 -0.01578605026062828 0.1473085304488828 0.1228982071662512 0.01872998258781158 -0.1785900122892218 0.1951703834948477 -1.192941569645306 -0.0939579019538087 0.0001567827502120462 -0.4787251165533745 -0.008484444296963056 0.4709297267384193 0.152301481842071 -0.04478787128468215 0.3179059990621557 -0.09029909775975833 -0.1565168825069901 -0.005808180533154668 0.3235581747931703 -0.4553071692990142 -0.8824585038506718 0.1875429624799867 -0.6 0.1375685300534709 0.5946328749565404 1.010483306506937 0.2236290707126681 -0.4626767897035591 -0.4154281207812159 0.07216546044934269 0.04664091931796911 -0.01562265823346419 0.1518103896891938 0.1410684327603446 0.03254818712486311 -0.1599841034427898 0.3823382959560468 -1.069323003209293 -0.08505202405033588 -0.9524438197880856 -0.6914172983845874 0.03545732015531298 0.4374115083008333 0.6729776148696531 0.2951079904479953 0.3536239921653395 0.04104791920597949 -0.104077556746444 0.0014469203209135 0.5409009382234703 -0.3811608762255578 -0.8902896102139697 0.3140688893743052 -0.61 0.1390961965573261 0.5950361507262838 1.0089893609118 0.2282355217599163 -0.4727690454037833 -0.4162538558881991 0.05822864057174749 0.03862611550468906 -0.01629835193539945 0.1560515206328226 0.1586563404686298 0.04743614733847957 -0.1387788806173859 0.5329368229320826 -0.9534996763729455 -0.08116784474324804 -1.810276901955217 -0.9172794357197804 -0.1604701939222216 0.4102849726077554 -0.01197592403839242 -0.1570332363748403 0.466982480480858 -0.09196629703560714 -0.1309926035681616 0.008617015272080966 0.2980522354597618 -0.4424362089237171 -0.8955956039181879 0.1092880486113742 -0.62 0.1406254863120026 0.5955681815136031 1.007708251593931 0.2341675053600796 -0.4818342140217657 -0.4170729346240199 0.03645278748575301 0.02818061117460635 -0.0186295447795057 0.1600054949850057 0.1413930829246262 0.05829919064004354 -0.1174168479893437 0.6474448065390591 -0.8639055867609198 -0.08372074981784709 -2.520250461467939 -1.177557401745116 -0.2956425082788814 0.3799801900116871 -0.696929462946438 -0.609174463197676 0.5803409687963766 -0.2249805132771938 -0.1579076503898791 0.01578711022324843 0.05520353269605328 -0.5037115416218764 -0.9009015976224063 -0.09549279215155691 -0.63 0.1419499219436277 0.5962132760409454 1.006643253826211 0.2409978956029429 -0.4900078809289391 -0.4179350790800987 0.008353913860590534 0.01495512549988653 -0.02156619004084111 0.1631668323556994 0.1300745578013876 0.07208692401952248 -0.09527792453939664 0.7115669349980881 -0.7597352126300441 -0.08719355616312589 -3.081766958213724 -1.469946908301054 -0.3037600051571838 0.2597703656495997 0.006360920042707584 -0.05725637217722347 0.1787292702611847 -0.2286588758162161 -0.1737452327424653 0.02700008951049306 0.4439336509461879 -0.3559427005366276 -0.8915983011597964 0.1076529616027089 -0.64 0.1433585877560501 0.5970372641778156 1.005808788723449 0.248257517919331 -0.4968070666334925 -0.4187865140425863 -0.02482740557150498 -0.001266470493458928 -0.02494621410602909 0.1653505639284484 0.1582391190840886 0.09407791254560363 -0.07131031499780498 0.7332912212440161 -0.5890093462369282 -0.08157885109958768 -3.536739622854544 -1.776779465470244 -0.3843182690493974 0.184459030422724 0.7096513030318531 0.494661718843229 -0.2228824282740073 -0.2323372383552385 -0.1895828150950515 0.03821306879773768 0.8326637691963226 -0.2081738594513789 -0.8822950046971868 0.3107987153569747 -0.65 0.1451757307790954 0.5980987723081311 1.005232464195969 0.2554262592974615 -0.5017075520642269 -0.4195939291986777 -0.06147085438953661 -0.02068264815639529 -0.0294627973252432 0.1671624700493777 0.2017717717075326 0.1174462963408425 -0.04347212219092423 0.694372144798038 -0.3915143180728589 -0.08025625426233336 -3.767270753544331 -2.116645557183666 -0.4774704098815653 0.1220568213118158 0.2544317776710859 0.1468907815616933 -0.1552710975747756 -0.3216111122228115 -0.1776473064251875 0.07001968788223124 0.6057962925666325 -0.2629436647325653 -0.8886860362327839 0.2301272111525245 -0.66 0.1473256689138521 0.5993706417610991 1.004948995645774 0.2620232626236111 -0.5046458845582082 -0.4203986806107059 -0.09967923289824279 -0.04380317143846507 -0.03366506300542321 0.166674392905658 0.2247981414263819 0.1361501770761015 -0.01273911954095882 0.6189436108478619 -0.1965787588863649 -0.08104610228695397 -3.849725560989453 -2.517648589296925 -0.3214547612425763 -0.2755376225071062 -0.2007877476896814 -0.2008801557198424 -0.08765976687554403 -0.4108849860903844 -0.1657117977553236 0.1018263069667248 0.3789288159369423 -0.3177134700137517 -0.8950770677683808 0.1494557069480743 -0.6699999999999999 0.1495184531917195 0.6007912339949351 1.004967330437898 0.2678452726842517 -0.5054707814531259 -0.42115190748776 -0.1383378545288701 -0.07140230356647399 -0.03718542891014526 0.1638298043045116 0.2090797455395786 0.1464626337026078 0.01542326153631261 0.5398684244069799 0.03551908022028891 -0.06953639150183712 -3.85929583324391 -3.007293484586088 -0.3714984498286886 -0.2992722940925738 -0.4890432313394873 -0.5703119305146432 -0.2522807745615176 -0.4678435331932149 -0.1145744894812924 0.1263078355217165 0.2465637763388209 -0.3037694087435338 -0.8457368400696879 0.06807211162855681 -0.6799999999999999 0.1514136844524935 0.602269781715382 1.005237804547908 0.2727088315742852 -0.5038571089474536 -0.4217881508083021 -0.176411090925278 -0.1040513540958106 -0.04087263260453399 0.1605711030963984 0.1652875380077199 0.1477412743983299 0.03768874403599495 0.4272533767264372 0.2911351212316158 -0.05764939098456474 -3.73264851414558 -3.527632269562465 -0.3548223191759123 -0.3583601439004663 -0.7772987149892934 -0.9397437053094441 -0.4169017822474911 -0.5248020802960454 -0.06343718120726129 0.1507893640767081 0.1141987367406995 -0.2898253474733158 -0.7963966123709949 -0.01331148369096068 -0.6899999999999999 0.1528661763000001 0.6037462468532233 1.005711185888552 0.2762774845296894 -0.4994485312391317 -0.422278807941699 -0.2126460432412297 -0.1420071973035079 -0.04302975553280865 0.154492325482559 0.1321918087746314 0.1486724659835553 0.05708137689812622 0.2843399816231598 0.6068372154954677 -0.03667947067813652 -3.503835428045359 -4.065597972654116 -0.09413118522346942 -0.8342714335823913 0.34903550148688 -0.0588048590613897 -0.3226649806174807 -0.3151120315331576 0.0481971760462984 0.1590221691986872 0.5762236121737838 -0.0880853230502487 -0.7113885441663835 0.2944074381460428 -0.7 0.1541971401736069 0.6052656452913255 1.006381309141978 0.2783528865521234 -0.4913952287362902 -0.4224456889215316 -0.2462776690661976 -0.1854045455624353 -0.04310583468389239 0.1443481533304514 0.1409819432277758 0.1563279344505044 0.07703712659243296 0.1286031901324107 1.020080080135523 0.007105839728283594 -3.211983215948846 -4.615933279808472 0.06138643626227194 -1.171439051554073 1.475369717963053 0.8221339871866647 -0.2284281789874702 -0.10542198277027 0.1598315332998581 0.1672549743206662 1.038248487606868 0.1136547013728184 -0.6263804759617723 0.6021263599830462 -0.71 0.1555670335609882 0.6068540760412318 1.007230214337123 0.2789152904468192 -0.478927172765841 -0.4220582607957108 -0.2767659440726542 -0.2343629342353405 -0.04184544247876096 0.1310535868732089 0.1178303499085938 0.1584616955077833 0.09100483380140149 -0.01193298631198003 1.465179057987497 0.07036164614459037 -2.880568125465046 -5.173146213144412 0.2107939817224566 -1.45914324580259 0.04704580839199866 -0.1275327404283643 -0.1314198091009183 -0.4152789098207147 0.1346387380809689 0.1751730409858878 0.4601783788196578 0.001729536725930942 -0.5269715433069267 0.2184899692993133 -0.72 0.1562504194849809 0.6083769488010219 1.008166624245302 0.2781980153235095 -0.4622586886958768 -0.4210388187844652 -0.3037869583779501 -0.2888155049927608 -0.03848791551027438 0.115731908296236 0.003680450550057549 0.1432163364272821 0.09453806919919988 -0.1273326134686635 1.860165700038503 0.1335086168132681 -2.518531075716695 -5.714769696711476 0.4808133889333012 -1.576861475500181 -1.381278101179056 -1.077199468043393 -0.03441143921436639 -0.7251358368711595 0.1094459428620797 0.1830911076511094 -0.1178917299675524 -0.1101956279209565 -0.4275626106520812 -0.1651464213844195 -0.73 0.1556309143398508 0.6097041216020246 1.009117332826406 0.2764299282647488 -0.4416965078898406 -0.419387305405947 -0.327043379655252 -0.3484322943029066 -0.03306090768910106 0.09736013344617293 -0.1149231601275553 0.1240519736362032 0.09639469840750207 -0.2251991658206974 2.259317089634914 0.1995180887147441 -2.131008435822983 -6.191417762701708 0.6081409325296152 -2.076469450751889 0.123861265532101 0.07177986849899087 -0.04506144059835272 -0.3940973051045562 0.2686592979114556 0.1838057645099021 0.4100513105139968 0.0766680473589316 -0.3337798408731897 0.2746182068867365 -0.74 0.1542051226714005 0.6108946632710051 1.01011033924117 0.2737157446563513 -0.4169314143338552 -0.4169939764136828 -0.3463722322159958 -0.4123004521944753 -0.02625404171511703 0.07462300015640891 -0.1575768541139783 0.1158901100228457 0.1029976359311934 -0.3165519233960226 2.700748250028306 0.2818717395624638 -1.733017332405084 -6.565043412996014 0.7567850194954416 -2.449933163440376 1.629000632243258 1.220759205041375 -0.05571144198233904 -0.06305877333795289 0.4278726529608315 0.1845204213686948 0.937994350995546 0.2635317226388197 -0.2399970710942982 0.7143828351578925 -0.75 0.1524521138974087 0.6120112325934377 1.011170222383876 0.270245070249408 -0.3877125703154618 -0.4136930941960441 -0.3619928483039865 -0.4793512192249627 -0.01754816549510934 0.05017861954181814 -0.2012301760993112 0.1050363750872326 0.1082056076708992 -0.3807595874244162 3.128793505478807 0.374217808671004 -1.393547393501304 -6.827437408325911 0.9581517831291225 -2.456612514218567 0.4098848909825314 0.2677585502120808 -0.1883252875049198 -0.4333903428820838 0.3586507888974771 0.1573115929203026 0.4962400108230781 0.136261878389146 -0.1878066013140547 0.4260120877650909 -0.76 0.1500164136411155 0.6129476431852208 1.012258983695806 0.2660370203192273 -0.3546400851877107 -0.4095913581461484 -0.3742920102521868 -0.5484957486654825 -0.007615774880074014 0.02513735877722172 -0.2941151505742708 0.07985836389294115 0.1087732697759502 -0.464027028043498 3.471476471899826 0.4420425060138498 -1.068726504446985 -6.983795895002458 1.002087898500968 -2.569309193441504 -0.8092308502781952 -0.6852421046172135 -0.3209391330275006 -0.8037219124262147 0.2894289248341226 0.1301027644719104 0.05448567065061025 0.008992034139472379 -0.1356161315338113 0.1376413403722893 -0.77 0.1465556325799564 0.6135902806144914 1.0133473373525 0.2609336477184592 -0.318636702674299 -0.4049347565907808 -0.3834067245895647 -0.6184384859029871 0.003006446970955175 -0.001567807532252603 -0.394007788088139 0.04894930987529177 0.109656415688573 -0.5520401394507392 3.713634751440265 0.48753732051874 -0.7581895709415643 -6.982954888990087 1.119510665192524 -2.682373304428598 -0.5896523325150596 -0.5543459916903112 0.1141025751559704 -0.6487982964879846 0.3497896715460233 0.107064704643974 0.03912459945450219 0.01374554321690032 -0.06717970603734537 0.214928766641003 -0.78 0.1422169233507407 0.613932233141009 1.014467291092091 0.2550883645834006 -0.2806786957457499 -0.3998754214265919 -0.3895352658292774 -0.6877189131751165 0.01471752229352963 -0.02672309203085508 -0.469700784185607 0.01972138334233544 0.1150932863554086 -0.6124091349015712 3.862401354927307 0.5225892277781246 -0.4714918849139443 -6.851333901927372 1.21985859281002 -2.259332831267136 -0.370073814751924 -0.4234498787634089 0.5491442833394414 -0.4938746805497545 0.4101504182579239 0.08402664481603755 0.02376352825839412 0.01849905229432827 0.001256719459120586 0.2922161929097167 -0.79 0.1373610668337119 0.6140058053121387 1.015658794348061 0.2485818508974338 -0.2416632559100685 -0.3945960722926364 -0.3928836016252829 -0.7550201459397827 0.02674389438509596 -0.0484056768457674 -0.4912115623090121 -0.0035421155210635 0.1227211654047379 -0.6945727837788144 3.936111207978978 0.5293829170283436 -0.2036243671223184 -6.589256042682639 1.179374836745968 -2.114761071942781 0.2923090242739885 0.1248582959903183 -0.05798467133354897 -0.386939840352496 0.4332994998633541 0.0544891340616147 0.3018494510853399 0.09600022727615781 0.0150743849810438 0.4351672327728761 -0.8 0.1325978712427834 0.6138906875024944 1.016911990411507 0.2410833252780835 -0.2020479796707702 -0.3893657167257177 -0.3937167350284271 -0.8191109018623047 0.03818419925330347 -0.06976985227425993 -0.4511685989655427 -0.01801661281245219 0.1274318478504326 -0.8108115215782956 3.982368635650686 0.5127905143707673 0.03154859365830223 -6.20923853349852 1.102645148138256 -2.19565095398318 0.9546918632999011 0.6731664707440455 -0.6651136260065393 -0.2800050001552374 0.4564485814687845 0.02495162330719186 0.5799353739122858 0.1735014022579874 0.02889205050296701 0.5781182726360355 -0.8100000000000001 0.1283557730645213 0.6136576380366775 1.01820290984894 0.2323274192172913 -0.1622978379729491 -0.3843776935832814 -0.3922890601891061 -0.8789487789186299 0.04922947669761063 -0.08714291034698694 -0.4022589695392695 -0.0289944509953327 0.1315609935764643 -0.9416940652249216 3.948666341423674 0.4806750626071021 0.2520416414121673 -5.745969549824982 1.100783897700422 -1.313146269609874 -0.07911364543863501 0.1213420904167511 -0.2095565240045568 -0.4322248737566937 0.3874702190583331 -0.008366931825891431 0.1531029703648742 0.05156055421830948 0.01858731431925226 0.2898741062141095 -0.8200000000000001 0.1244525331949497 0.6133027750696997 1.019559389361841 0.2222229564806891 -0.1234545200920937 -0.3798349965037635 -0.3887147968560584 -0.9337829462999743 0.06008734834685685 -0.09671648984741049 -0.3833969372274674 -0.04237931304462968 0.1405438629438704 -1.08052285674031 3.801003872257558 0.4237253017870698 0.4608662924036571 -5.20849659850237 1.065163989126067 -0.6357552395224857 -1.112919154177171 -0.4304822899105432 0.2460005779974257 -0.5844447473581499 0.3184918566478818 -0.04168548695897472 -0.2737294331825373 -0.0703802938213684 0.008282578135537516 0.001629939792183451 -0.8300000000000001 0.1207653104985306 0.6128172543588253 1.021026565047679 0.2106591706211578 -0.08643603420840922 -0.3759901010987995 -0.38311954068132 -0.9830130007248858 0.07009611349473088 -0.1021153770974882 -0.3451049516928281 -0.05359081189458766 0.1526353732347228 -1.232281942143898 3.604202458494612 0.3451224903726351 0.65610337911482 -4.63442104188967 0.9315782143906804 -0.3991869857989506 -0.2340807874326725 -0.009230437207683628 -0.06208642006165701 -0.3623366725321148 0.3516539964882166 -0.07545538269404321 0.002254723911492568 0.01565291034714405 0.01743684896281632 0.2499085714051731 -0.8400000000000001 0.1177292871683637 0.612253639176521 1.022606978806756 0.1975763650982524 -0.05134028784189554 -0.3729351724729727 -0.3756343605423456 -1.026409502245962 0.07861869611350936 -0.1038035250695074 -0.253157063977039 -0.05799820733062626 0.1631914775918841 -1.384326789415114 3.416455968823406 0.2657319459596448 0.8388510852508879 -4.041786017735332 0.7679274833069565 0.1063926160892058 0.6447575793118262 0.412021415495176 -0.3701734181207397 -0.1402285977060797 0.3848161363285515 -0.1092252784291117 0.2782388810055225 0.1016861145156565 0.02659111979009514 0.4981872030181628 -0.85 0.1157175654387785 0.6116660440020937 1.024279875702871 0.1829598437113956 -0.01811111266561341 -0.3706705067225688 -0.3663780814894297 -1.063818801961742 0.08607471580083734 -0.09991060499591123 -0.1556454656809681 -0.05982946718027887 0.1708091586196979 -1.539380064840612 3.225897256019951 0.1866567983185756 1.007557089953304 -3.442085441496332 0.7214534073926299 0.6429666437858358 -0.0675309064921891 0.1236603847127198 -0.2909293678028099 -0.3061470227791667 0.2904730793004686 -0.1429639064919325 0.07331587136884943 0.04430832463880176 0.01829497251189583 0.2548884351589972 -0.86 0.1144872141799246 0.6110508770404066 1.026011587118924 0.1667807122637531 0.01310802107024119 -0.3692129246226523 -0.3555801714508595 -1.095291441397403 0.09301130332604154 -0.09152868729067487 -0.09688296983080685 -0.06351256478256823 0.1749543815795739 -1.696848801572268 3.014447680737902 0.1043152158621769 1.147177282381767 -2.854453961711777 0.6640410508822024 1.004192142417237 -0.7798193922962043 -0.1647006460697364 -0.2116853174848801 -0.4720654478522537 0.1961300222723857 -0.1767025345547533 -0.1316071382678236 -0.01306946523805299 0.009998825233696516 0.01158966729983158 -0.87 0.1137676176949607 0.6104004166561889 1.027770555978329 0.1490605339309768 0.04216538256115012 -0.368579389960759 -0.3434608715629254 -1.120983709366777 0.09874111753803706 -0.08018811110020857 -0.04306724061748322 -0.06592656528417665 0.1764846594213723 -1.840027434002349 2.801147817173876 0.02458828278335934 1.273495480446389 -2.287692242343428 0.4971261282903502 1.252367023597919 -0.1329283951041865 0.1988145807223641 -0.1660701192540563 -0.228661922835813 0.2237090436708046 -0.2041609542581997 0.0203512186188696 0.02445696658138758 0.01246252810513213 0.184749075932301 -0.88 0.1137052510984649 0.6097454046702588 1.02953418568975 0.1301233522033189 0.06921344140831871 -0.3686772276416476 -0.3301740061371048 -1.141119138447881 0.1032579126273176 -0.0667124682602785 0.03456300786282352 -0.06442288512506848 0.1758865519826018 -1.940249480548603 2.612587151989835 -0.04195925269419375 1.380690389959065 -1.743086184057813 0.4214372263392133 1.431205472309987 0.5139626020878314 0.5623298075144646 -0.1204549210232325 0.01474160218062778 0.2512880650692235 -0.2316193739616461 0.1723095755055628 0.06198339840082815 0.01492623097656774 0.3579084845647704 -0.89 0.1144361108632008 0.6091134339291673 1.03127657031664 0.1103331588720059 0.09448429324392746 -0.3693779053603467 -0.3158949076051857 -1.155899220162379 0.1072630640868229 -0.05186557727685732 0.1058111528626726 -0.06247943982968588 0.1718730831983884 -2.0166058542842 2.44368296146409 -0.0968435722915429 1.469283475359271 -1.215993917684208 0.3701892175242394 1.522317657615268 0.06437512747388406 0.303415642550862 -0.1400738424011925 -0.1410774256095233 0.155675212655783 -0.2697741285831805 0.07982778239060979 0.03510260671532997 0.01117697949490857 0.213541449761368 -0.9 0.1157055183112847 0.6084856523389365 1.032957301549775 0.0898149017462305 0.1181290955642442 -0.3705874447123966 -0.3009052534512291 -1.165500291978415 0.1104736200170502 -0.03658321644315273 0.1422725445324289 -0.06358505495288819 0.1635558732313691 -2.085862239441096 2.287377248931428 -0.1437315793643411 1.522801514366569 -0.7072842043655296 0.2625181204836088 1.518299442366668 -0.3852123471400632 0.04450147758725929 -0.1596927637791526 -0.2968964533996744 0.06006236024234252 -0.3079288832047147 -0.01265401072434323 0.008221815029831778 0.007427728013249391 0.06917441495796553 -0.91 0.1172235871024434 0.6078355345375294 1.034538347557341 0.06868874831811994 0.140273732082719 -0.3722203250002342 -0.2855653724948589 -1.170095971811857 0.1129995550161982 -0.02172929915909798 0.1614163428058771 -0.06653321523974148 0.1524402108522142 -2.133722964054366 2.143453437775069 -0.1810071268849727 1.541372782079729 -0.2132202222267448 0.2456342572510395 1.444799477572691 -0.1742799912573838 0.06392112757735081 -0.1225235584055473 -0.2154971219321685 0.04094450522024393 -0.3422892053329752 0.02639057549324816 0.01684237476447966 0.007974403517982061 0.1377453895117937 -0.9199999999999999 0.1189353477495336 0.6071530938359173 1.036001843418225 0.04725335210767675 0.1610362319799755 -0.3741708402237318 -0.2701538357061899 -1.169792065621026 0.1154456127201721 -0.007840917629131172 0.1810109157187375 -0.07004963499388019 0.140045843894941 -2.147710795907626 2.010949924687762 -0.2072585664963248 1.537132680826302 0.2726330004890957 0.2465426614488065 1.325192291549078 0.03665236462529565 0.08334077756744233 -0.08535435303194194 -0.1340977904646627 0.02182665019814532 -0.3766495274612358 0.06543516171083956 0.02546293449912755 0.00852107902271473 0.2063163640656218 -0.9299999999999999 0.1208344489388722 0.6064340398176219 1.037336027342647 0.0258075764229481 0.1805010799822527 -0.3763429327064511 -0.2548492827740486 -1.164651111810757 0.117518346843042 0.004659840259777764 0.197178992395203 -0.07370333544730841 0.1265573219940642 -2.13887768632676 1.881835088952129 -0.2259897115260532 1.520201267386288 0.7553943887373128 0.1793296540649554 1.171928469538039 -0.008112412750995168 0.1009911570059436 -0.05924571188734623 -0.1297566336013952 -0.01531735141576782 -0.4081618860124566 0.05471497650907713 0.02107127529726214 0.00832692905438232 0.1787616408937755 -0.9399999999999999 0.1228463210023619 0.6056801837913293 1.038528317478201 0.004527131475368304 0.1986692420227072 -0.3786672300838227 -0.2398213431227776 -1.154687445302829 0.1192587156202668 0.01553703542773628 0.2035650905489366 -0.07701003659330613 0.1116670861216644 -2.114644648477855 1.751612732323202 -0.2376995454267573 1.481810024652221 1.237175540020979 0.1800695923197891 1.000479747358994 -0.05287719012728598 0.1186415364444449 -0.03313707074275052 -0.1254154767381277 -0.05246135302968097 -0.4396742445636774 0.04399479130731469 0.01667961609539674 0.00813277908604991 0.1512069177219291 -0.95 0.1248803767091076 0.6048932568920231 1.039566890193264 -0.01643949209317766 0.2155249865949174 -0.3810761446457464 -0.225287447430934 -1.139907453396592 0.1210191099508642 0.02464364142412556 0.2026599873740351 -0.08047766651885835 0.09607824023978515 -2.077220985791171 1.618452210293076 -0.2435169960273585 1.423667680585123 1.719291626257228 0.1730736256233973 0.8210577040323308 -0.04231995079021381 0.07798065852874363 0.0103226514685316 -0.1097341342551336 -0.08431283788590092 -0.4794472394967044 0.03975885886836201 0.01380735219738355 0.008621830931193583 0.1561965324713864 -0.96 0.1268877994813193 0.6040685839959338 1.040450497949976 -0.03698810665165188 0.2310166067920535 -0.3835262425857575 -0.211374018173703 -1.120292237077068 0.122741475169209 0.0319625145506523 0.1982385036421319 -0.08455923594991425 0.08067409445152655 -2.031042846463504 1.478787857308389 -0.2459362210442388 1.357716737729666 2.204220422678436 0.1724637698692816 0.642933173386484 -0.03176271145314163 0.03731978061304238 0.05378237367981371 -0.0940527917721394 -0.1161643227421209 -0.5192202344297314 0.03552292642940932 0.01093508829937036 0.009110882776337256 0.1611861472208437 -0.97 0.1288302042395445 0.603200598287382 1.041180741891493 -0.05702908726614057 0.2450769958911709 -0.3859808494434148 -0.1981425817720806 -1.095819496475694 0.1246322578961767 0.03750694997003364 0.1895347280130928 -0.08906956142033447 0.06540534030484958 -1.975827599485337 1.331837588413057 -0.2443522588179585 1.288817261564436 2.69019663978934 0.1944245179147343 0.4662559592393288 -0.04330727821591299 0.005617364385462989 0.03886146476301237 -0.08573867756876602 -0.1505950691212306 -0.5649627261655982 0.02227707412885465 0.004211889907320617 0.009534260647864109 0.159326240057246 -0.98 0.1306643396417846 0.6022865596543292 1.041759217685132 -0.07647814510238075 0.257624311078274 -0.3884006299287314 -0.1855927385630218 -1.066490925437422 0.1264046003753133 0.04129367471042977 0.1765846324451064 -0.09376982085013325 0.05032046487607091 -1.912658290813801 1.176173074905533 -0.2389709465760859 1.221398099216952 3.175386510057921 0.1487757203030498 0.2913910375894407 -0.05485184497868435 -0.0260850518421164 0.02394055584621102 -0.07742456336539262 -0.1850258155003403 -0.6107052179014649 0.009031221828299976 -0.002511308484729127 0.009957638519390964 0.1574663328936483 -0.99 0.1323276344105936 0.6013260433181709 1.042185941634046 -0.09529072998984341 0.268593660672095 -0.3907397627599435 -0.1737304167643004 -1.032321421946749 0.1283215444127008 0.04333519007049676 0.1537155613045805 -0.09820600312168556 0.03493469188988311 -1.852450686381366 1.018037341528908 -0.2278358870091666 1.150618546985963 3.657781150271126 0.220474314247505 0.1165340185016442 -0.2430594935104093 -0.07907179079356634 0.001753692943516143 -0.1608523461080278 -0.2421005138969391 -0.6536446725710688 -0.006655002734471577 -0.010722505744758 0.0104392374579893 0.0957103330145679 -1 0.1336914756676343 0.6003249884570927 1.042456118862596 -0.1135789988240609 0.2779918678622569 -0.3929369530157711 -0.1625893218941299 -0.9933499631881105 0.1305313112017243 0.04361679476201642 0.1166939300914724 -0.1018775258340547 0.01901112080332155 -1.807795080164758 0.8619445941737062 -0.2105824314991642 1.07715271350676 4.135777563650939 0.207340270630241 -0.06059109612002574 -0.4312671420421344 -0.1320585297450163 -0.02043316995917873 -0.244280128850663 -0.299175212293538 -0.6965841272406729 -0.02234122729724313 -0.01893370300478687 0.01092083639658764 0.03395433313548746 +0.5 0.1182693924521629 0.5874513142954801 1.03432792288309 0.2831883168029904 -0.316642510195574 -0.4051086501814696 -0.3088667043167284 0.01979134418315417 0.01132245422027419 0.0991210471579605 0.1104096086636304 -0.05420643883761736 -0.1839990879550372 -1.158528774537583 -1.781657506849771 -0.1184190756667737 7.362991366561018 0.8921116314746302 -0.7844009993911758 0.4744040282154014 -0.02028564647263131 -0.006789605924556891 0.001506927488818214 -0.006167307609859925 -0.08521562711577976 0.02609200483901056 0.004866867065752426 -0.4830583668846514 -0.823666754373493 0.1455164385037524 +0.51 0.1194411271874417 0.5869710026973075 1.032420863862036 0.2715465977216752 -0.334445678198135 -0.4062649430193804 -0.2367856048195064 0.0283976366791121 0.003899864577170024 0.1039606868458395 0.1235343137962737 -0.04230642617930774 -0.1970039202561978 -1.161207718433244 -1.776090967381845 -0.1131159517258832 7.026259851093204 0.8250170800376584 -0.6953964649907353 0.4931930700681445 -0.02016126681167719 -0.1199093276630423 0.04166352706765103 -0.03539668091816239 -0.09343003729568185 0.0110617557237418 0.03092490559063766 -0.526584750077072 -0.8623082651076168 0.1756651315794135 +0.52 0.1207320182361712 0.5865961748642454 1.030396020397955 0.2601363089001699 -0.3521066270175991 -0.4073764984121975 -0.1688808809306678 0.03620909003032141 -0.002491065794762073 0.1089782917734782 0.1342408713537602 -0.03310968581556832 -0.207555976560371 -1.112242722575612 -1.753213670230376 -0.10947158664804 6.527716244884354 0.7331438025249063 -0.578069145156762 0.5099970761673386 0.009166298250581229 -0.1730215857432449 0.07084640585410966 -0.007318814672041192 -0.09594726166066704 0.02064943014482486 0.0242434902474684 -0.5219792293902132 -0.868289339041502 0.1684194994139564 +0.53 0.1221157129241009 0.5863035164828018 1.028280828791136 0.2494734952586193 -0.3694458013616279 -0.4084586402446282 -0.1067616894469396 0.04297452025699863 -0.007603582964939822 0.1141502673643323 0.141896246060525 -0.02556285107312186 -0.2148838330541982 -1.012648444979861 -1.711527274483919 -0.1070699622692074 5.872329845582426 0.6159264176054163 -0.4439275570592234 0.5236734378672203 -0.02926110915894834 -0.1759471428071797 0.07767327718517408 -0.08307539663689013 -0.1254176700458664 0.003280746044442499 0.08573606230862947 -0.5466884926141504 -0.900843504215761 0.1759763671051049 +0.54 0.1235579067539486 0.5860821006307835 1.026110313971857 0.2400367712156651 -0.3862752940254483 -0.4095201613062034 -0.05191012814459663 0.0484473018783273 -0.01135948229955525 0.1194372684480974 0.1459406997373635 -0.01886117993051961 -0.2186206190522128 -0.8670248028563821 -1.651277334188722 -0.1053474324769268 5.074190208607294 0.4746140814551971 -0.3067455780442979 0.5330021747495454 -0.04531159705054289 -0.1557091382158788 0.0710891340484026 -0.09489214306377633 -0.1441087637748772 0.01108450356684909 0.09589822992810393 -0.530110242104218 -0.90276706133854 0.1503057307342534 +0.55 0.1250222454268064 0.585924657038343 1.023921588562518 0.232263072476993 -0.4024160215858657 -0.4105654542522402 -0.00568290453408785 0.05239487633022863 -0.0137774522784712 0.124791916831469 0.1464821143794899 -0.01258514353026788 -0.2184642374130956 -0.6831514588461159 -1.574720069503956 -0.1036302848712209 4.15659273663786 0.3120286979956052 -0.1809725593649297 0.5368751086124963 -0.07341870863226141 -0.1323486170499219 0.09696926200761731 -0.1445194208867977 -0.1669964987330484 -0.005916106362621705 0.1430835803314639 -0.5619895921354349 -0.9435990110123393 0.1588057843819099 +0.5600000000000001 0.1264786506324443 0.5858312456607243 1.021754233731648 0.226465011759381 -0.4177267332477112 -0.4115911495664435 0.03092849105102848 0.05463043361965017 -0.01506141631937461 0.1301537227541019 0.1443540062933988 -0.006054736966154875 -0.2143465033581804 -0.4718971986443781 -1.485274154474325 -0.1014279061102148 3.151024603528803 0.1322106489592407 -0.07994439044179047 0.534433682601813 -0.09811809439012679 -0.08663054735526175 0.1013976459604099 -0.1366775925937161 -0.1768097126686248 0.002678537940233573 0.1264466901755218 -0.5624258527447256 -0.9626966553971628 0.1334064434475064 +0.5700000000000001 0.1279047201622132 0.5858067832633895 1.019648497689696 0.222874594001782 -0.4320933765643826 -0.4125912937363294 0.05716750768756159 0.0549963075115442 -0.01549728117762364 0.1354575068332389 0.1408913077410652 0.001390291097563234 -0.2061790695777011 -0.246079410704692 -1.38730617309769 -0.09848798941471691 2.095360673468428 -0.06063142479380419 -0.01411061250577 0.5251175693303334 -0.1070098902968102 -0.05836752155353692 0.1746691527010094 -0.1555128062018813 -0.1845972724547404 -0.01233777828802738 0.1602984054690309 -0.5998665236453298 -1.012693031800696 0.1471150234252864 +0.58 0.1292971049488791 0.5858636121546428 1.017643085049184 0.2215455623887016 -0.4454578899944197 -0.4135586505856934 0.0728073435142092 0.05338589403950669 -0.01548126919544509 0.1406319628628038 0.1376170576727876 0.01020352075143129 -0.1942818230704399 -0.01961996974066497 -1.284848177147471 -0.09487044200586009 1.031188441551703 -0.2630468238270684 0.0104309776437242 0.5085680726874052 -0.1003411827683271 -0.02365271116214066 0.2193635466535562 -0.1283549792878417 -0.1796660078371326 -0.001467489517609944 0.1515534614634851 -0.5846674608001606 -1.021950129033063 0.1275024523794385 +0.59 0.1306623616017786 0.5860171022399266 1.015774109476325 0.2224377245909383 -0.4577864820351594 -0.4144878447638619 0.07790567800053697 0.04971096074713963 -0.01543360678079225 0.1456037519851563 0.1359029893158643 0.02078440574342522 -0.179012787184246 0.1938568291369061 -1.18073735425554 -0.09099503406626679 0.0006818392582265842 -0.4734255507199096 -0.007091650620785264 0.484373432597918 -0.066592981860256 -0.01517838584865092 0.343713957798547 -0.1264188033723469 -0.1709534548699076 -0.01367258396287177 0.2073217524464626 -0.5937256705813063 -1.050309687955311 0.144022234796197 +0.6 0.1320245390633717 0.5862850984582726 1.01407283939184 0.2253387873504177 -0.4690699795446318 -0.4153790840757869 0.07306504797026131 0.04388766870363718 -0.01574696532601339 0.1502911051310585 0.1370012194115201 0.03310474736381976 -0.1607407253956361 0.3821601417078849 -1.075829270893987 -0.08727946875714795 -0.9566044617689782 -0.6927185740541442 -0.06177321433107229 0.4516808773973334 0.001013554840332997 -0.002433685312995396 0.4097566919220816 -0.06259014868247091 -0.1354826332938117 0.01116474179329989 0.2126544704711924 -0.5567793279664089 -1.043155375293704 0.1326275215271911 +0.61 0.133411890973314 0.5866839300487021 1.012566908184708 0.229960968462155 -0.4792983506863266 -0.4162367089928117 0.05913706004930872 0.03581591491220017 -0.01676250788483748 0.1546043264399594 0.1410104994704309 0.04686719501146975 -0.1401061518878044 0.5352345725559853 -0.9688616694240871 -0.0844359538635346 -1.806841690361429 -0.9248494600133671 -0.1434525713005654 0.4087774864877889 0.04011514772975058 -0.03938104259796579 0.4815777029781454 -0.06413315580446058 -0.1195311839340521 0.004933664286522134 0.2754229154120095 -0.553702210342251 -1.067516589131792 0.1473036219661722 +0.62 0.1348555757906501 0.5872265548442895 1.011277503636944 0.2359026486398658 -0.488427547172696 -0.4170716119373724 0.03737124280423501 0.02532633398776768 -0.01865836222936109 0.1584229369029128 0.1482678008902615 0.06186338839540564 -0.1174353935218558 0.6460599549025856 -0.8559943398289226 -0.08273507426434316 -2.524170326593203 -1.176284000653242 -0.2378355714709754 0.3527587082078224 0.05222964408007594 -0.07536445471734038 0.4666337714315908 -0.07864460600850651 -0.1228578074800959 0.02221625785835246 0.2954900987789668 -0.5017388162845015 -1.05442864501475 0.1387911863979601 +0.63 0.1363873220992474 0.5879247340502036 1.010222739660328 0.2427261258890033 -0.4963812234192905 -0.4178957754730558 0.009159566290977158 0.01219824408505325 -0.02150635219811463 0.1616027004614009 0.158278056905847 0.07784874297016738 -0.09329406681550105 0.71079688354784 -0.7316804398449551 -0.08224790650405177 -3.092157890310382 -1.455455047674034 -0.3283630762316256 0.2794275670094175 -0.03889320679779443 -0.179925974372455 0.3147737345860392 -0.2220313451586008 -0.1727243100397662 0.01532460812944018 0.3628218836879437 -0.4808320731711931 -1.059192072655782 0.1495830856250893 +0.64 0.1380250688502997 0.5887850555073484 1.00941608900035 0.2499618140832809 -0.5029999465766948 -0.4187195755400873 -0.02395177328701308 -0.00390518832140093 -0.02515763683303146 0.1639361595134937 0.1694678893812608 0.09429183864156124 -0.06781273019446653 0.7285021439306055 -0.5890037219908925 -0.08266238053393353 -3.504102939539679 -1.771352501401194 -0.3984945047036326 0.1834978069287546 -0.1732837167711983 -0.2589783472567243 0.08039376832919667 -0.3569217456286459 -0.2254390685849227 0.03766788231977526 0.3507439660670119 -0.4320289663586636 -1.032811348842764 0.1360748748189212 +0.65 0.1397800877756958 0.5898118182417216 1.008872300258255 0.2571463072582493 -0.5080736869239786 -0.4195472800903514 -0.06040270751187733 -0.023379930846896 -0.02935754068288314 0.165177671595726 0.1810421672238006 0.1108394408081348 -0.04090918201136339 0.7017423820460935 -0.4201909907206181 -0.08249870011367402 -3.761954098504372 -2.131941011522886 -0.4341266266702764 0.05915440852910674 -0.2395348879843248 -0.2744075233461059 -0.1996259426282386 -0.4732461472821987 -0.2436698846351417 0.04401928361677689 0.3817756368274506 -0.4305910417686014 -1.050087317857719 0.1607159718253828 +0.66 0.1416360376252929 0.5909974189750122 1.008598622084383 0.2638635795438634 -0.511292699256202 -0.4203619529542566 -0.09870825911852443 -0.0467109087083599 -0.03369297659450844 0.1650062436643039 0.1896540742214807 0.1260594384250498 -0.01379061654998174 0.6350579660597508 -0.2180581189788011 -0.08005604326216582 -3.875026415896249 -2.542599568594959 -0.4256009170583549 -0.09909019580212383 -0.2057925088476897 -0.2246356089803944 -0.4311144136006219 -0.4432207263970571 -0.1893297409173543 0.08360716107884145 0.3472291240794222 -0.3910034085249908 -1.03117459977973 0.1529608555294416 +0.6699999999999999 0.1435525048833716 0.5923234977588169 1.008593466859828 0.269736080398297 -0.5123010554187084 -0.4211322097019154 -0.1374665276217202 -0.07440722251753736 -0.03770431859373234 0.1630671634034331 0.1920820868023458 0.1386493746531425 0.01239093824254806 0.5354725368311128 0.02374742035455817 -0.07264686483852302 -3.85794251749348 -3.004452592710003 -0.3689567213062378 -0.2952990710013728 -0.08310206630353693 -0.1645706559034951 -0.4739539854949392 -0.4030883860257928 -0.1099772191647623 0.09730415004029523 0.3940582116499921 -0.3545362836052571 -1.005273035408436 0.190992689964958 +0.6799999999999999 0.1454465335495014 0.5937602675944201 1.008839068181309 0.2744936369205684 -0.51067054017157 -0.4217879214224059 -0.1754934141234056 -0.1069557485519493 -0.04091789579102943 0.1589687979516896 0.1851663558316911 0.1481976487847538 0.03636069265740403 0.4120690996273239 0.3097161629501775 -0.05714703782852103 -3.728750015594191 -3.513042013641858 -0.2660479566729926 -0.5309472339766581 -0.005470228028169538 -0.09746270464232376 -0.4191587468329881 -0.3404961124873608 -0.03637266483425318 0.1253926606228307 0.3792414950646218 -0.2696891410390236 -0.9128230428141781 0.1848864919230322 +0.6899999999999999 0.1472139062788022 0.5952770192768375 1.00930795501041 0.277931575055632 -0.505951240529939 -0.4222373934526599 -0.2117349525985962 -0.1447938261226078 -0.04289108129849007 0.1523202733061319 0.1657385329416524 0.1544289491143461 0.05682147831798594 0.2749239374992519 0.6412773565585128 -0.0305313418166321 -3.507838505750983 -4.057759099453135 -0.1237399893886732 -0.8038467276042519 0.02206360031603261 -0.09147001210244701 -0.2986610192803203 -0.3818583117348875 0.003988946728204206 0.1180344034203989 0.4241044174830835 -0.2119179756010173 -0.8368199273089133 0.2211803492466295 +0.7 0.148709911066598 0.59683437180502 1.009963593850774 0.279980223872831 -0.4977023212167495 -0.4223542277306256 -0.2454158007645663 -0.1881746425202781 -0.04329571247019362 0.1427900827502171 0.1308927675306806 0.1563178179377992 0.07371109491008197 0.1342112360544341 1.015640097261894 0.009380512629124085 -3.21661195375008 -4.621589779044223 0.04766291047842913 -1.107280416048093 0.06617161490875485 -0.05800767676104455 -0.1299685491099664 -0.3667912685051712 0.05732063105016971 0.1344612952437713 0.4171672117146763 -0.1201806250157321 -0.7227403194983915 0.2247151254899642 +0.71 0.1497743899037884 0.5983857883867301 1.010767660551998 0.2806354415149368 -0.4855163420353597 -0.4220008190771777 -0.2759057919664524 -0.2372172531418746 -0.0418808677244457 0.1301078432651267 0.07931181272734297 0.1531022606269541 0.08647487696116217 -0.001081428670784594 1.425332477526302 0.06344827299930286 -2.876652704596956 -5.181850835784351 0.2361032033182383 -1.429478796020033 0.1049672200900051 -0.04848829116406929 -0.04866315190779746 -0.4150477314504754 0.1063209330248338 0.1265690630291494 0.4631096091641966 -0.0570255650846898 -0.6228056134881681 0.2682928424242615 +0.72 0.1502423235775438 0.5998791522620136 1.011680544022524 0.2800003208786653 -0.4691201368960188 -0.421042321171863 -0.3028541832159016 -0.2917100290461506 -0.03855770511088719 0.1141942805288155 0.01158373484368369 0.1447072766524865 0.0954744487704467 -0.1238564196209973 1.857685288852125 0.1303983630024487 -2.508291963262688 -5.711622835580132 0.4293264840405455 -1.753545066292246 0.09957314874868138 -0.04094662503328975 0.03461933576351798 -0.4163235928255903 0.1607022916908585 0.1415347325285191 0.4222065519302488 0.01845111891304544 -0.4873671381798043 0.2694889530230737 +0.73 0.149963660666231 0.6012650452072841 1.012665532316753 0.2782164489464846 -0.448340154248128 -0.4193584448604668 -0.3260401616574838 -0.3512509667274267 -0.03331770882304717 0.09509964062073269 -0.06860748180097898 0.1317615039814542 0.1011415913927743 -0.2301693155033073 2.295901245849135 0.2070944139940334 -2.129694176968155 -6.182016428661606 0.6159129053685104 -2.057986571571835 0.04456631249231247 -0.05520777325183152 -0.02945819044595752 -0.4947239092977977 0.2004141681781332 0.1231217601642371 0.4023832996644543 0.03746349401608326 -0.3919469945149401 0.3100669975035742 +0.74 0.1488443506476295 0.6025001861732397 1.01369574347673 0.2754519075948364 -0.4232503118765742 -0.4168860825976379 -0.3454638757935527 -0.4150593921791131 -0.02629464436669597 0.07318247597242852 -0.1565456866140541 0.1145568807895592 0.1045190219201794 -0.319990303514468 2.719657233584722 0.2880955532889721 -1.755839102160025 -6.5651203896622 0.7859401177427817 -2.318050014336519 0.01579473971329005 -0.04663972722896676 -0.09560086385590603 -0.5033782686162904 0.2465562876271935 0.1201178854896003 0.3443052642222039 0.07344596456372714 -0.2882161483713889 0.306105374351044 +0.75 0.1468305636878391 0.6035432382465024 1.014752517086762 0.271862871258864 -0.3940669726502979 -0.4136031100245219 -0.3612131297580719 -0.4821768265081283 -0.01768274490005969 0.04897423667583859 -0.2450459210635825 0.09369707629482456 0.1067789106694516 -0.3966101785297775 3.107734941391359 0.3669352025169638 -1.397796544323638 -6.836869582549848 0.9313258244434532 -2.507921576114284 -0.01892754553560536 -0.03348407851738777 -0.1682126933042428 -0.562151606010209 0.2783952005782427 0.07988028959322362 0.320705062955209 0.05732428886019805 -0.2293336186414594 0.3492998212907147 +0.76 0.143966747911967 0.6043669985477736 1.015830185901785 0.2675438397272453 -0.3612811264543302 -0.4095786537236434 -0.373495503751622 -0.5513668459582906 -0.007770406900647266 0.02333756982748637 -0.3265514498303573 0.07069852639128474 0.1086980629186284 -0.4659893426437394 3.440158627523021 0.436392298841486 -1.062463107966204 -6.979637413891599 1.046027824298011 -2.603735524688976 -0.06469533966664934 -0.02423564932630185 -0.191496109582337 -0.5538540568784074 0.3011176732621705 0.05921758581476053 0.2546075266110679 0.0702124674510638 -0.1456571105093296 0.3297795287661307 +0.77 0.1403499718499178 0.6049554846700731 1.016925790137232 0.2625418619536154 -0.3255069532872541 -0.4049290804466119 -0.382551208158428 -0.6213008662137324 0.003120730787359984 -0.002718036128105385 -0.3934161894927887 0.0470933571359243 0.1106032804480249 -0.5356772795355931 3.700955717888945 0.4900247112832445 -0.752963665799591 -6.984109120017833 1.126159113705721 -2.586965205537312 -0.08904073941748047 -0.01570576508860817 -0.1305255702776421 -0.5876083312629644 0.3131547677982107 0.01505284166509243 0.2423698880696201 0.05368266491852283 -0.08297830685105606 0.3608353205241517 +0.78 0.136166175583845 0.6053107588718384 1.018045861436291 0.2568048727874664 -0.2875364178566162 -0.3998481124036104 -0.3886404949157058 -0.6905878780150685 0.01463196338151284 -0.02799332506541876 -0.4399554906350928 0.02405614228444365 0.1135914756410617 -0.6129916211475407 3.879431080235395 0.522671252035448 -0.4691795780605647 -6.850235723070419 1.170046805527133 -2.447672121033301 -0.06140619864366038 0.003484394285898637 -0.03863033198242248 -0.5338056553973725 0.3294675840774756 0.002210197016807415 0.2265781428434483 0.09001207866248027 -0.003948754417062531 0.332802778440645 +0.79 0.1316320450074933 0.6054413667471996 1.019200810199125 0.2502339657369588 -0.2482111914433269 -0.3945577545007216 -0.3920142762145242 -0.7578679209471527 0.02640081964905878 -0.05125646780828673 -0.462662015774126 0.002518996046486708 0.1177486954420284 -0.7040482860955706 3.971808761200643 0.5313213552706113 -0.2089642537955611 -6.58694985784993 1.178190964322215 -2.186842978377268 -0.06811010709404632 0.01737912815036478 -0.01848166737087338 -0.5396471136936859 0.3334910257663594 -0.0380495791677375 0.2179642085523342 0.06379329335403446 0.01344747130366871 0.352495160389194 +0.8 0.126997107445585 0.6053702100579418 1.020407843715459 0.2426667371227235 -0.20837635145704 -0.3893032647636323 -0.3928875314334661 -0.8219504150621364 0.0380851129947602 -0.07136791564970341 -0.4601168877464047 -0.01629677063935293 0.1240084263411448 -0.8122559338930601 3.981353794834912 0.5154976188755426 0.03092563791471999 -6.210725959650333 1.153134221158228 -1.817333140743042 -0.09560725293342945 0.02161104622934026 -0.01072864707850616 -0.4881149468565089 0.3260174543444028 -0.05698362919629731 0.1648884234177944 0.07380048943609856 0.02643341973955813 0.3154936304015006 +0.8100000000000001 0.1225122105819975 0.6051274741925546 1.021687598441382 0.2339270929889133 -0.1688328025207545 -0.3843246349587643 -0.3914523755105991 -0.8817768322935244 0.04936605406846736 -0.08731326293549385 -0.4334529588160635 -0.03165303792026813 0.1321704741620593 -0.9382674025336406 3.917325880814984 0.4770442817608389 0.2536501317471174 -5.743492590269558 1.099184579089288 -1.362974493827976 -0.1660096277595645 0.01693115194791585 -0.08171788359774601 -0.5039320453428022 0.3081564899662982 -0.1043400096810564 0.114993630846938 0.02527180973026191 0.005931849458479434 0.3246691676725099 +0.8200000000000001 0.1183962387923644 0.6047490965898926 1.023055812305071 0.2238494988787575 -0.1302304360728843 -0.3798260603351597 -0.3878636370967545 -0.9365989689403739 0.05999141628666767 -0.09845216907452159 -0.3863318729555387 -0.04342511809433824 0.141700253894359 -1.079845929162184 3.793117297151853 0.4194865826228866 0.4616421361102902 -5.209869842742639 1.022018450056857 -0.8560449113905005 -0.1910681093186272 0.01227342066252778 -0.132579613852084 -0.4432262563746579 0.2928707943063284 -0.1235862239400789 0.06453040427804084 0.05344718173402536 0.0307109744492242 0.2795823886959195 +0.8300000000000001 0.1148384624706067 0.6042704548363214 1.024522461975496 0.2122909430984929 -0.09311812212346132 -0.3759819519979313 -0.3822638974505868 -0.9858367877368294 0.06975054459880461 -0.1043814617889449 -0.3235397126680301 -0.05183049896161619 0.1515701600388611 -1.232701032035724 3.624659912915266 0.3478966635334877 0.6560307848135768 -4.634387052726369 0.9281606262932998 -0.3333209799936742 -0.203386175985142 0.05426797488204385 -0.1963969642100873 -0.4385108803164291 0.2732856955471643 -0.1700140686838325 0.04982151140151736 0.02094879339796087 0.01604190012920148 0.28738195540372 +0.8400000000000001 0.111959118113563 0.6037219412838261 1.026086025102814 0.199178762135143 -0.05783094941092832 -0.3728968954902762 -0.3747885215666762 -1.02922057271846 0.07852169709098915 -0.1051887356443853 -0.2506454800127593 -0.05739947787916811 0.1610829452729136 -1.390570965779251 3.428089049773857 0.2676762167082113 0.8367693836588744 -4.039063079777589 0.8244232860663806 0.1683588604060831 -0.1856555473475136 0.08765751006612998 -0.2239985568583454 -0.373769964895791 0.2467971109524616 -0.190832115799639 0.03401487111445522 0.05667110767433179 0.02729931832094429 0.2442715767756875 +0.85 0.1098400835222414 0.6031304179294597 1.027737953680343 0.1844848994113749 -0.02459851434698223 -0.3706380320438522 -0.3655779953149116 -1.066615967625166 0.08622847885668795 -0.1011993367035521 -0.1732630729177953 -0.06063345700323907 0.1689169558321688 -1.546751787770569 3.218244163628755 0.1843728585232601 1.002449750830411 -3.441824662046435 0.717340728967557 0.6164237686908399 -0.1842647639722022 0.1429306339946628 -0.2307118450393033 -0.3680618428226167 0.2163793180189198 -0.2364036878417054 0.03401868432460019 0.02025709553583149 0.01242436321797706 0.2507001359525764 +0.86 0.1084918239618824 0.602514706863579 1.029456647931444 0.168272722203808 0.006530857873914248 -0.3692039106008775 -0.3547972488673409 -1.098093241169043 0.09287666490822372 -0.09312220345196291 -0.09649047382022556 -0.06223702018197369 0.1744360799873684 -1.694233862539001 3.007476481163932 0.102727816018367 1.150813422820094 -2.855438807211834 0.6127041432337691 0.9859057225572825 -0.1484876965428949 0.171516272465863 -0.1939613801238536 -0.2987644967293255 0.1863920563223518 -0.2503968535797731 0.03109223346799911 0.05660586246306282 0.02350262698304716 0.2107214379540572 +0.87 0.1078925979441194 0.6018889763036855 1.031214963475143 0.1506504389241407 0.03558095337896829 -0.3685638691683106 -0.3426291143605187 -1.123790529577826 0.098507464877591 -0.08180264100470624 -0.02470495192218703 -0.06285192052512382 0.1766671657184552 -1.826875685005261 2.804854839382941 0.02668478512048099 1.278972509058759 -2.287598610292934 0.5152229806870429 1.261485387575484 -0.1396551816696162 0.2077448366221024 -0.1690715328007197 -0.2917422709147714 0.1540300341765436 -0.2896796115777154 0.03733748925058632 0.01718004281981994 0.008786381270339814 0.2178871127970793 +0.88 0.1079707204796426 0.6012588118785085 1.032978793985131 0.1318021506714868 0.06267419905229454 -0.3686421286059583 -0.3292946180758772 -1.143916808089868 0.1032164671229117 -0.0682229232868245 0.03897923683703706 -0.06312379323868957 0.1755390732450117 -1.939434857136302 2.616106514818378 -0.04093235802454346 1.38408577838396 -1.741236827863836 0.4283445984244435 1.437936776682297 -0.1102943322613668 0.205555724490978 -0.1179891715527941 -0.2381073486261448 0.1157671170453044 -0.3042749044963656 0.03844967144442124 0.05371890894743139 0.02006734048943668 0.1841985657093324 +0.89 0.1086371855742267 0.6006262185728599 1.034713383312808 0.1119416452451965 0.08796006119232372 -0.3693476588319285 -0.3150330928798917 -1.158686831721447 0.1071178800149448 -0.05336705865780861 0.09244899921423683 -0.06348800029174517 0.170861174062951 -2.028408560285084 2.443734463548503 -0.09837064894627802 1.463752913950065 -1.215703381756011 0.3542252241152515 1.519250310379348 -0.1141923264720233 0.214607857741736 -0.1251374449305266 -0.2489588502206757 0.07431047111362318 -0.3508129870655529 0.0457804470301762 0.01350705284135404 0.005908722660014549 0.1942586666219542 +0.9 0.1097824048066165 0.5999871892246592 1.036385665101842 0.09131913282251856 0.1116022593304857 -0.3705734808204197 -0.3001088867341373 -1.168289585391068 0.1103467164878781 -0.03811763385406813 0.1347300643981745 -0.06441100174911679 0.1630775655165401 -2.091836256413823 2.287373714444945 -0.1449907105287528 1.51662196833776 -0.7077828354720553 0.2938293146044729 1.516648811627215 -0.09423866870207667 0.1862192340669662 -0.1166025655162444 -0.2102421867258637 0.03159906893882206 -0.3733918876936332 0.0441210865145892 0.04998482800807955 0.01766513097788165 0.167023019033209 +0.91 0.1112938366432303 0.5993355420915223 1.037966200713361 0.07019094580667778 0.1337537032628804 -0.3722112047382383 -0.2847916152848227 -1.1728867310661 0.113040711443784 -0.02326237768161409 0.1658600855623127 -0.06605154384661216 0.1526775761051685 -2.129833013989682 2.144477642137698 -0.180956244869621 1.542582970350758 -0.2130411535728487 0.2471073778161904 1.446046451207326 -0.105406533749042 0.1705888344190699 -0.1283803354251437 -0.2284171134194601 -0.01060906008462062 -0.4229720293198352 0.0490785140238882 0.009856002960938106 0.004233824259280103 0.1815029579805871 +0.9199999999999999 0.1130656821706174 0.5986634959683602 1.039432177010305 0.04880183519801818 0.1545230635753135 -0.3741606491545052 -0.2693422143506103 -1.172578305543294 0.1153316180689914 -0.009363824263047093 0.1868128025528557 -0.06849079975412189 0.140165702601447 -2.144020974977588 2.010956990452615 -0.2073348102184239 1.543047865317322 0.2733314040956946 0.2132116484647617 1.325308260849806 -0.09654379157019882 0.1221285666818433 -0.1131198604384311 -0.1980902960996096 -0.05153780672533942 -0.4417418497083322 0.04329413175528866 0.04601079020090384 0.01656403640015422 0.1579248278931524 +0.9299999999999999 0.1150007605196527 0.5979636935100082 1.040764102506482 0.0273801559089761 0.1739873244370345 -0.3763304322326438 -0.2540074267872508 -1.167433277182129 0.1173432928402976 0.003136534148649528 0.1990337139256125 -0.07157674434777581 0.1260639520579467 -2.137479517396283 1.881847571181662 -0.2255382499107277 1.520762066306806 0.7555422011556944 0.1907450136470462 1.171760996080909 -0.108923089279972 0.095153320715561 -0.07817436404552551 -0.2103212851365733 -0.08647797168611022 -0.4853195654561517 0.04451605480516672 0.005980845432747344 0.003763378537225061 0.1743021397064493 +0.9399999999999999 0.1170229733825578 0.5972298200327752 1.041950347159941 0.006108952158783347 0.1921590627847484 -0.3786497430427483 -0.2389899246454293 -1.157470102859811 0.1191789524989438 0.01401134715039899 0.2042397053268048 -0.07530500353028095 0.1110295340576447 -2.113925867207723 1.752452487651212 -0.2372403566115972 1.479590781009721 1.236960596326268 0.1780086259327661 1.000199178860374 -0.09176455916545291 0.05322469010889749 -0.02716300827780393 -0.1685234874667908 -0.1174975006365024 -0.5036771150203715 0.03372524052994869 0.04017453299466033 0.01655477929217862 0.1548423332088002 +0.95 0.1190664080840329 0.5964556803704022 1.042983197229665 -0.0148554075184051 0.2090212594053758 -0.3810585799512208 -0.2244639265397696 -1.142689029313647 0.1209299485397139 0.02312064405453451 0.2036387863343389 -0.0795948528076473 0.09554363251472969 -2.077287931695356 1.618900968527071 -0.2438156522741142 1.42400520587602 1.719578671228562 0.1732532881474006 0.8216033098618035 -0.1020686443771505 0.02724356940238606 0.01331759367100656 -0.1729767026292627 -0.1480289785542369 -0.5547405017000215 0.03041712077993393 -0.000471446154649741 0.004147227523465427 0.1718053909101177 +0.96 0.1210795801365671 0.5956364844993559 1.043861282862788 -0.03540364374443165 0.2245153647963461 -0.3835118286320548 -0.210541893212833 -1.123072038268799 0.1226652723804148 0.03044227550553655 0.1981871755386214 -0.08431624526493725 0.08007664673761856 -2.030701176975341 1.478834241719797 -0.2461227110838952 1.358797825265095 2.204144096063106 0.1748741859189336 0.6426660882336782 -0.09797228291070582 -0.006621384178210546 0.04938046674767348 -0.1419535694395239 -0.182530384279782 -0.5827992914668885 0.01692674867685024 0.03084358690976733 0.01724680580404551 0.1529223084316032 +0.97 0.1230147693854941 0.5947687428953569 1.044585774431909 -0.05544569379788843 0.2385729198143569 -0.3859680029758309 -0.1973045054844944 -1.09860068551119 0.1244439540989245 0.03598437505304809 0.1880366801902627 -0.08924798937301973 0.06486159996916693 -1.976848948627283 1.331596924415613 -0.2443824488961097 1.288536129551067 2.690077947722015 0.181520387996337 0.4660828209531034 -0.1313538433197888 -0.03978196753462914 0.05647940402811182 -0.1651777133608105 -0.2206058902305895 -0.6410522970071767 0.0139707948016305 -0.009516990338775925 0.005088596456005776 0.1664528473642173 +0.98 0.1248240338592418 0.5938512064351327 1.045159313519823 -0.07492342501520244 0.251125706535323 -0.3883848834344726 -0.1847740424388431 -1.069271449469095 0.1263088447446081 0.03977051191828804 0.1730022205027434 -0.09427521650995567 0.04988615049631563 -1.917837409746784 1.177880582310851 -0.2382639340569996 1.217412888727622 3.175720752960115 0.1921159713537275 0.2914735517793615 -0.1429409547671137 -0.072435725670184 0.05241373219279104 -0.1489611044351683 -0.258694330226255 -0.674069922357827 0.0002250065021015505 0.01895097855246542 0.01837318535446604 0.1456270009799206 +0.99 0.1264571860073696 0.5928844266568041 1.045584351463705 -0.09379158527324724 0.2621135356806388 -0.3907167828943385 -0.1729464898672887 -1.035095551705594 0.1282965039620668 0.04181356264376812 0.152903123741994 -0.0989221784856926 0.03508667118980149 -1.855536424584973 1.019175124389458 -0.2272142658334721 1.149054900792289 3.658380790185348 0.2058381059532268 0.1167055742459352 -0.1560614966867928 -0.04655753169554448 0.02722162797641263 -0.1562973517211928 -0.2877053709406546 -0.7330289412303957 0.003924729652349164 -0.01928120367959122 0.006410551212495123 0.1537619528926732 +1 0.1278675946264652 0.5918759340788197 1.045860351601814 -0.1120289891613579 0.2714990065758555 -0.3929111349094881 -0.1617737989188168 -0.9961253938564845 0.1304340515399773 0.04209600302179241 0.1284535146962922 -0.1026177764411664 0.02007858934167715 -1.791686135759945 0.8574089322910401 -0.2107544451137603 1.086440564111099 4.134572770081629 0.2220936434441039 -0.06064851771179479 -0.1209492803901133 0.00501065214779533 -0.006779734193959766 -0.110083962961585 -0.3054189216354929 -0.7566298831571111 0.0007787834775830574 0.009161734651005938 0.02003445069708584 0.1315972131303141 diff --git a/OpenSim/Moco/Test/testMocoConstraints.cpp b/OpenSim/Moco/Test/testMocoConstraints.cpp index bbfe7daf23..a3cd00ce05 100644 --- a/OpenSim/Moco/Test/testMocoConstraints.cpp +++ b/OpenSim/Moco/Test/testMocoConstraints.cpp @@ -1741,10 +1741,12 @@ TEMPLATE_TEST_CASE("MocoOutputBoundConstraint", "", problem.setStateInfo("/slider/position/value", MocoBounds(-5, 5), MocoInitialBounds(-1, 1)); - problem.setStateInfo("/slider/position/speed", {-1, 1}); + problem.setStateInfo("/slider/position/speed", {-5, 5}); problem.setStateInfo("/slider2/position/value", MocoBounds(-5, 5), MocoInitialBounds(-1, 1)); - problem.setStateInfo("/slider2/position/speed", {-1, 1}); + problem.setStateInfo("/slider2/position/speed", {-5, 5}); + problem.setControlInfo("/actuator", {-10, 10}); + problem.setControlInfo("/actuator2", {-10, 10}); auto* constr = problem.addPathConstraint(); Sine lower; diff --git a/OpenSim/Moco/Test/testMocoTrack.cpp b/OpenSim/Moco/Test/testMocoTrack.cpp index 94b98b362d..4d1adc3669 100644 --- a/OpenSim/Moco/Test/testMocoTrack.cpp +++ b/OpenSim/Moco/Test/testMocoTrack.cpp @@ -50,28 +50,49 @@ TEST_CASE("MocoTrack gait10dof18musc", "[casadi]") { MocoTrack track; - track.setModel(ModelProcessor("walk_gait1018_subject01.osim") | + auto modelProcessor = ModelProcessor("walk_gait1018_subject01.osim") | ModOpRemoveMuscles() | ModOpAddReserves(100) | - ModOpAddExternalLoads("walk_gait1018_subject01_grf.xml")); - track.setStatesReference( - TableProcessor("walk_gait1018_state_reference.mot") | - TabOpLowPassFilter(6)); + ModOpAddExternalLoads("walk_gait1018_subject01_grf.xml"); + auto tableProcessor = TableProcessor("walk_gait1018_state_reference.mot") | + TabOpLowPassFilter(6); + + track.setModel(modelProcessor); + track.setStatesReference(tableProcessor); track.set_states_global_tracking_weight(10.0); + track.set_control_effort_weight(0.001); track.set_track_reference_position_derivatives(true); track.set_initial_time(0.5); track.set_final_time(1.0); MocoStudy study = track.initialize(); auto& solver = study.updSolver(); - solver.set_optim_constraint_tolerance(1e-4); - solver.set_optim_convergence_tolerance(1e-4); + solver.set_optim_constraint_tolerance(1e-6); + solver.set_optim_convergence_tolerance(1e-6); MocoSolution solution = study.solve(); solution.write("testMocoTrackGait10dof18musc_solution.sto"); - const auto actual = solution.getControlsTrajectory(); + // Check that the solution is close to the tracking reference (unit test). + Model model = modelProcessor.process(); + model.initSystem(); + auto table = tableProcessor.processAndConvertToRadians(model); + table.trim(0.5, 1.0); + GCVSplineSet splines(table); + const auto& time = solution.getTime(); + SimTK::Vector timeVec(1, 0.0); + for (const auto& label : table.getColumnLabels()) { + const auto& solState = solution.getState(label); + SimTK::Vector error(time.size(), 0.0); + for (int i = 0; i < (int)time.size(); ++i) { + timeVec[0] = time[i]; + error[i] = splines.get(label).calcValue(timeVec) - solState[i]; + } + CHECK(error.norm() < 1e-2); + } + + // Check that the solution is close to the tracking reference (regression). + const auto actual = solution.getStatesTrajectory(); MocoTrajectory std("std_testMocoTrackGait10dof18musc_solution.sto"); - const auto expected = std.getControlsTrajectory(); - CHECK(std.compareContinuousVariablesRMS( - solution, {{"states",{}}}) < 1e-2); + const auto expected = std.getStatesTrajectory(); + CHECK(std.compareContinuousVariablesRMS(solution) < 1e-2); } From dae3a81b102378f5ef53b95330e6937b85f45f3f Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 13 Sep 2024 14:27:02 -0700 Subject: [PATCH 35/38] Add CMake build flag for FATROP --- dependencies/CMakeLists.txt | 49 +++++++++++++++++++++---------------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index 33a74b1b40..cac81ba0ec 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -243,6 +243,13 @@ if (OPENSIM_WITH_CASADI) CMAKE_DEPENDENT_OPTION(SUPERBUILD_casadi "Automatically download, configure, build and install casadi" ON "OPENSIM_WITH_CASADI" OFF) mark_as_advanced(SUPERBUILD_casadi) + + option(BUILD_FATROP_WITH_CASADI "Build the FATROP optimization library along with CasADi" ON) + + # TODO remove when Windows builds are fixed. + if (WIN32) + set(BUILD_FATROP_WITH_CASADI OFF) + endif() endif() if (WIN32) @@ -356,24 +363,24 @@ endif() if(SUPERBUILD_casadi) if (WIN32) AddDependency(NAME casadi - DEFAULT ON - DEPENDS ipopt - GIT_URL https://github.com/casadi/casadi.git - GIT_TAG 3.6.6 - CMAKE_ARGS -DWITH_IPOPT:BOOL=ON - -DIPOPT_LIBRARIES:FILEPATH=ipopt.dll.lib - -DIPOPT_INCLUDE_DIRS:PATH=${CMAKE_INSTALL_PREFIX}/ipopt/include/coin-or - -DLIB_FULL_ipopt.dll.lib:FILEPATH=${CMAKE_INSTALL_PREFIX}/ipopt/lib/ipopt.dll.lib - -DWITH_MUMPS:BOOL=OFF - -DWITH_BUILD_FATROP:BOOL=ON - -DWITH_BUILD_BLASFEO:BOOL=ON - -DWITH_FATROP:BOOL=ON - -DWITH_THREAD:BOOL=ON - -DWITH_BUILD_MUMPS:BOOL=OFF - -DWITH_EXAMPLES:BOOL=OFF - -DPKG_CONFIG_USE_CMAKE_PREFIX_PATH:BOOL=ON - -DCMAKE_PREFIX_PATH:PATH=${CMAKE_INSTALL_PREFIX}/ipopt - ) + DEFAULT ON + DEPENDS ipopt + GIT_URL https://github.com/casadi/casadi.git + GIT_TAG 3.6.6 + CMAKE_ARGS -DWITH_IPOPT:BOOL=ON + -DIPOPT_LIBRARIES:FILEPATH=ipopt.dll.lib + -DIPOPT_INCLUDE_DIRS:PATH=${CMAKE_INSTALL_PREFIX}/ipopt/include/coin-or + -DLIB_FULL_ipopt.dll.lib:FILEPATH=${CMAKE_INSTALL_PREFIX}/ipopt/lib/ipopt.dll.lib + -DWITH_MUMPS:BOOL=OFF + -DWITH_BUILD_FATROP:BOOL=ON + -DWITH_BUILD_BLASFEO:BOOL=${BUILD_FATROP_WITH_CASADI} + -DWITH_FATROP:BOOL=${BUILD_FATROP_WITH_CASADI} + -DWITH_THREAD:BOOL=${BUILD_FATROP_WITH_CASADI} + -DWITH_BUILD_MUMPS:BOOL=OFF + -DWITH_EXAMPLES:BOOL=OFF + -DPKG_CONFIG_USE_CMAKE_PREFIX_PATH:BOOL=ON + -DCMAKE_PREFIX_PATH:PATH=${CMAKE_INSTALL_PREFIX}/ipopt + ) else() AddDependency(NAME casadi DEFAULT ON @@ -383,9 +390,9 @@ if(SUPERBUILD_casadi) CMAKE_ARGS -DWITH_IPOPT:BOOL=ON -DWITH_THREAD:BOOL=ON -DWITH_BUILD_MUMPS:BOOL=OFF - -DWITH_BUILD_FATROP:BOOL=ON - -DWITH_BUILD_BLASFEO:BOOL=ON - -DWITH_FATROP:BOOL=ON + -DWITH_BUILD_FATROP:BOOL=${BUILD_FATROP_WITH_CASADI} + -DWITH_BUILD_BLASFEO:BOOL=${BUILD_FATROP_WITH_CASADI} + -DWITH_FATROP:BOOL=${BUILD_FATROP_WITH_CASADI} -DWITH_EXAMPLES:BOOL=OFF -DPKG_CONFIG_USE_CMAKE_PREFIX_PATH:BOOL=ON -DCMAKE_PREFIX_PATH:PATH=${CMAKE_INSTALL_PREFIX}/ipopt From 151fed01f98e737dfb71744e4518ade1d69d7b5a Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 13 Sep 2024 15:06:10 -0700 Subject: [PATCH 36/38] Silence stdext::checked_array_iterator warnings --- cmake/OpenSimMacros.cmake | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/cmake/OpenSimMacros.cmake b/cmake/OpenSimMacros.cmake index f24eacd046..e1b9e537e9 100644 --- a/cmake/OpenSimMacros.cmake +++ b/cmake/OpenSimMacros.cmake @@ -221,10 +221,9 @@ function(OpenSimAddLibrary) # This target links to the libraries provided as arguments to this func. target_link_libraries(${OSIMADDLIB_LIBRARY_NAME} ${OSIMADDLIB_LINKLIBS}) - target_compile_options(${OSIMADDLIB_LIBRARY_NAME} PUBLIC - # disable warning 4996 on Windows: `spdlog` transitively - # uses a deprecated `stdext::checked_array_iterator` - $<$:/wd4996> + target_compile_definitions(${OSIMADDLIB_LIBRARY_NAME} PUBLIC + # `spdlog` transitively uses a deprecated `stdext::checked_array_iterator` + $<$:_SILENCE_STDEXT_ARR_ITERS_DEPRECATION_WARNING> ) # This is for exporting classes on Windows. From d860f8791f08899c3dbcaaab4fe16131ef975fed Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 13 Sep 2024 16:17:04 -0700 Subject: [PATCH 37/38] Remove control interpolation option from MocoCasADiSolver --- .../Moco/MocoCasADiSolver/CasOCFunction.cpp | 4 ++-- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 3 --- .../Moco/MocoCasADiSolver/MocoCasADiSolver.h | 10 --------- OpenSim/Moco/MocoDirectCollocationSolver.cpp | 1 - OpenSim/Moco/MocoDirectCollocationSolver.h | 10 --------- OpenSim/Moco/MocoInverse.cpp | 1 - OpenSim/Moco/MocoTropterSolver.cpp | 4 ++++ OpenSim/Moco/MocoTropterSolver.h | 5 +++++ OpenSim/Moco/Test/testMocoConstraints.cpp | 6 ++++- OpenSim/Moco/Test/testMocoInverse.cpp | 4 ++-- doc/Moco/MocoStudy.dox | 22 +++++++++++-------- 11 files changed, 31 insertions(+), 39 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp index f4ad5dc4dc..0a5adfdb0b 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCFunction.cpp @@ -205,7 +205,7 @@ casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, } else if (i == 4) { return fullPoint.at(derivatives)(Slice(), 0); } else if (i == 5) { - return fullPoint.at(final_time)(0); + return fullPoint.at(final_time)(-1); } else if (i == 6) { return fullPoint.at(states)(Slice(), -1); } else if (i == 7) { @@ -215,7 +215,7 @@ casadi::DM Endpoint::getSubsetPoint(const VariablesDM& fullPoint, } else if (i == 9) { return fullPoint.at(derivatives)(Slice(), -1); } else if (i == 10) { - return fullPoint.at(parameters)(Slice(), 0); + return fullPoint.at(parameters)(Slice(), -1); } else if (i == 11) { // TODO: We should find a way to actually compute the integral // from fullPoint. Or, make the integral an optimization variable. diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index d55380d862..c33b2a1f2a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -48,13 +48,10 @@ void MocoCasADiSolver::constructProperties() { constructProperty_optim_finite_difference_scheme("central"); constructProperty_parallel(); constructProperty_output_interval(0); - constructProperty_minimize_implicit_multibody_accelerations(false); constructProperty_implicit_multibody_accelerations_weight(1.0); constructProperty_minimize_implicit_auxiliary_derivatives(false); constructProperty_implicit_auxiliary_derivatives_weight(1.0); - - constructProperty_enforce_path_constraint_mesh_interior_points(false); constructProperty_minimize_state_projection_distance(true); constructProperty_state_projection_distance_weight(1e-6); constructProperty_projection_slack_variable_bounds({-1e-3, 1e-3}); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h index d741671474..43948554c7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h @@ -142,7 +142,6 @@ class OSIMMOCO_API MocoCasADiSolver : public MocoDirectCollocationSolver { OpenSim_DECLARE_PROPERTY(optim_finite_difference_scheme, std::string, "The finite difference scheme CasADi will use to calculate problem " "derivatives (default: 'central')."); - OpenSim_DECLARE_OPTIONAL_PROPERTY(parallel, int, "Evaluate integral costs and the differential-algebraic " "equations in parallel across grid points? " @@ -154,7 +153,6 @@ class OSIMMOCO_API MocoCasADiSolver : public MocoDirectCollocationSolver { "indicates no intermediate trajectories are saved, 1 indicates " "each iteration is saved, 5 indicates every fifth iteration is " "saved, etc."); - OpenSim_DECLARE_PROPERTY(minimize_implicit_multibody_accelerations, bool, "Minimize the integral of the squared acceleration continuous " "variables when using the implicit multibody mode. " @@ -171,14 +169,6 @@ class OSIMMOCO_API MocoCasADiSolver : public MocoDirectCollocationSolver { "The weight on the cost term added if " "'minimize_implicit_auxiliary_derivatives' is enabled." "Default: 1.0."); - - OpenSim_DECLARE_PROPERTY(enforce_path_constraint_mesh_interior_points, bool, - "If the transcription scheme is set to 'hermite-simpson' or one of " - "the pseudospectral schemes (e.g., 'legendre-gauss-3', " - "'legendre-gauss-radau-3'), then enable this property to enforce " - "MocoPathConstraints at points interior to the mesh interval. " - "Default: false."); - OpenSim_DECLARE_PROPERTY(minimize_state_projection_distance, bool, "Minimize the distance between the projection states and the " "constraint manifold when using the 'Bordalba2023' method for " diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.cpp b/OpenSim/Moco/MocoDirectCollocationSolver.cpp index 5388f91243..aa09243f5c 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.cpp +++ b/OpenSim/Moco/MocoDirectCollocationSolver.cpp @@ -25,7 +25,6 @@ void MocoDirectCollocationSolver::constructProperties() { constructProperty_mesh(); constructProperty_verbosity(2); constructProperty_transcription_scheme("hermite-simpson"); - constructProperty_interpolate_control_mesh_interior_points(true); constructProperty_enforce_constraint_derivatives(true); constructProperty_multibody_dynamics_mode("explicit"); constructProperty_optim_solver("ipopt"); diff --git a/OpenSim/Moco/MocoDirectCollocationSolver.h b/OpenSim/Moco/MocoDirectCollocationSolver.h index e26a8889ec..600a062424 100644 --- a/OpenSim/Moco/MocoDirectCollocationSolver.h +++ b/OpenSim/Moco/MocoDirectCollocationSolver.h @@ -96,7 +96,6 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "The number of uniformly-sized mesh intervals for the problem " "(default: 100). If a non-uniform mesh exists, the non-uniform " "mesh is used instead."); - OpenSim_DECLARE_PROPERTY(verbosity, int, "0 for silent. 1 for only Moco's own output. " "2 for output from CasADi and the underlying solver (default: 2)."); @@ -107,13 +106,6 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "the number of collocation points per mesh interval between 1 and " "9), and 'legendre-gauss-radau-#' for Legendre-Gauss-Radau " "transcription."); - OpenSim_DECLARE_PROPERTY(interpolate_control_mesh_interior_points, bool, - "If the transcription scheme is set to 'hermite-simpson' or one of " - "the pseudospectral schemes (e.g., 'legendre-gauss-3', " - "'legendre-gauss-radau-3') then enable this property to constrain " - "the control values at mesh interior points to be linearly " - "interpolated from the control values at the mesh interval " - "endpoints. Default: true."); OpenSim_DECLARE_PROPERTY(multibody_dynamics_mode, std::string, "Multibody dynamics are expressed as 'explicit' (default) or " "'implicit' differential equations."); @@ -155,14 +147,12 @@ class OSIMMOCO_API MocoDirectCollocationSolver : public MocoSolver { "enforced, set the bounds on the slack variables performing the " "velocity correction to project the model coordinates back onto " "the constraint manifold. Default: [-0.1, 0.1]"); - OpenSim_DECLARE_PROPERTY(implicit_multibody_acceleration_bounds, MocoBounds, "Bounds on acceleration variables in implicit dynamics mode. " "Default: [-1000, 1000]"); OpenSim_DECLARE_PROPERTY(implicit_auxiliary_derivative_bounds, MocoBounds, "Bounds on derivative variables for components with auxiliary " "dynamics in implicit form. Default: [-1000, 1000]"); - OpenSim_DECLARE_PROPERTY(kinematic_constraint_method, std::string, "The method used to enforce kinematic constraints in the direct " "collocation problem. 'Posa2016' for the method by Posa et al. " diff --git a/OpenSim/Moco/MocoInverse.cpp b/OpenSim/Moco/MocoInverse.cpp index e0d092bec0..81f278c161 100644 --- a/OpenSim/Moco/MocoInverse.cpp +++ b/OpenSim/Moco/MocoInverse.cpp @@ -104,7 +104,6 @@ std::pair MocoInverse::initializeInternal() const { // ------------------------- auto& solver = study.initCasADiSolver(); solver.set_multibody_dynamics_mode("implicit"); - solver.set_interpolate_control_mesh_interior_points(false); solver.set_minimize_implicit_auxiliary_derivatives(true); solver.set_implicit_auxiliary_derivatives_weight(0.01); solver.set_optim_convergence_tolerance(get_convergence_tolerance()); diff --git a/OpenSim/Moco/MocoTropterSolver.cpp b/OpenSim/Moco/MocoTropterSolver.cpp index 0f17c0ba58..163d208933 100644 --- a/OpenSim/Moco/MocoTropterSolver.cpp +++ b/OpenSim/Moco/MocoTropterSolver.cpp @@ -35,6 +35,7 @@ void MocoTropterSolver::constructProperties() { constructProperty_optim_jacobian_approximation("exact"); constructProperty_optim_sparsity_detection("random"); constructProperty_exact_hessian_block_sparsity_mode(); + constructProperty_interpolate_control_midpoints(true); } bool MocoTropterSolver::isAvailable() { @@ -165,6 +166,9 @@ MocoTropterSolver::createTropterSolver( get_exact_hessian_block_sparsity_mode()); } + dircol->set_interpolate_control_midpoints( + get_interpolate_control_midpoints()); + // Get optimization solver to check the remaining property settings. auto& optsolver = dircol->get_opt_solver(); diff --git a/OpenSim/Moco/MocoTropterSolver.h b/OpenSim/Moco/MocoTropterSolver.h index fb8e64c2f5..df9bb0e71a 100644 --- a/OpenSim/Moco/MocoTropterSolver.h +++ b/OpenSim/Moco/MocoTropterSolver.h @@ -85,6 +85,11 @@ class OSIMMOCO_API MocoTropterSolver : public MocoDirectCollocationSolver { "property must be set. Note: this option only takes effect when " "using " "IPOPT."); + OpenSim_DECLARE_PROPERTY(interpolate_control_midpoints, bool, + "If the transcription scheme is set to 'hermite-simpson', then " + "enable this property to constrain the control values at mesh " + "interval midpoints to be linearly interpolated from the control " + "values at the mesh interval endpoints. Default: true."); MocoTropterSolver(); diff --git a/OpenSim/Moco/Test/testMocoConstraints.cpp b/OpenSim/Moco/Test/testMocoConstraints.cpp index a3cd00ce05..568a54fb9b 100644 --- a/OpenSim/Moco/Test/testMocoConstraints.cpp +++ b/OpenSim/Moco/Test/testMocoConstraints.cpp @@ -1584,7 +1584,6 @@ TEST_CASE("Prescribed kinematics with kinematic constraints", "[casadi]") { auto& solver = study.initCasADiSolver(); solver.set_num_mesh_intervals(10); solver.set_multibody_dynamics_mode("implicit"); - solver.set_interpolate_control_mesh_interior_points(false); MocoSolution solution = study.solve(); const auto Fx = solution.getControl("/forceset/force_x"); const auto Fy = solution.getControl("/forceset/force_y"); @@ -2229,3 +2228,8 @@ TEST_CASE("ConstantAccelerationConstraint") { } } +// TODO enable test for Windows when MSVC build issues in FATROP are fixed. +TEST_CASE("FATROP solver", "[casadi][unix]") { + auto model = createDoublePendulumModel(); + +} diff --git a/OpenSim/Moco/Test/testMocoInverse.cpp b/OpenSim/Moco/Test/testMocoInverse.cpp index b8e5a18152..a7dd23f9f7 100644 --- a/OpenSim/Moco/Test/testMocoInverse.cpp +++ b/OpenSim/Moco/Test/testMocoInverse.cpp @@ -186,7 +186,8 @@ TEST_CASE("MocoInverse Rajagopal2016, 18 muscles", "[casadi]") { auto& problem = study.updProblem(); // Add control bound constraint. - auto* controlBound = problem.addPathConstraint(); + auto* controlBound = + problem.addPathConstraint(); controlBound->addControlPath("/forceset/med_gas_r"); controlBound->addControlPath("/forceset/med_gas_l"); controlBound->setLowerBound(Constant(0)); @@ -194,7 +195,6 @@ TEST_CASE("MocoInverse Rajagopal2016, 18 muscles", "[casadi]") { auto& solver = study.updSolver(); solver.resetProblem(problem); - solver.set_enforce_path_constraint_mesh_interior_points(true); MocoSolution solution = study.solve(); diff --git a/doc/Moco/MocoStudy.dox b/doc/Moco/MocoStudy.dox index 4524406fad..2ca4fe5456 100644 --- a/doc/Moco/MocoStudy.dox +++ b/doc/Moco/MocoStudy.dox @@ -281,15 +281,6 @@ also supports both non-holonomic constraints (that is, \f$ \nu(q, u, p) = 0 \f$) and acceleration-only constraints (that is, \f$ \alpha(q, u, \dot u, p) = 0 \f$), but these constraints are less commonly used in OpenSim models. -By default, the controls within the mesh interval interior are constrained by -linear interpolation of control mesh endpoint values and is the recommended setting. -However, you may disable this behavior in the solver: - -\code{.cpp} -MocoCasADiSolver& solver = moco.initCasADiSolver(); -solver.set_interpolate_control_mesh_interior_points(false); -\endcode - @section casadivstropter MocoCasADiSolver versus MocoTropterSolver Moco provides two direct collocation solvers that transcribe continuous optimal @@ -312,6 +303,19 @@ Those distributing Moco as a dependency of closed-source software may prefer distributing Moco without CasADi, as CasADi’s “weak copyleft” GNU Lesser General Public License 3.0 places requirements on how CasADi is redistributed. +CasADi and Tropter's direct collocation implementations have differed slightly +over time as CasADi has become the primary focus of recent development. However, +for problems that can be solved with both solvers, the solutions should be +similar. The property `interpolate_control_midpoints` has been removed from +MocoCasADiSolver since it is unnecessary in our implementation of the +Hermite-Simpson transcription scheme. For legacy reasons, it is still enabled +by default in MocoTropterSolver, but you may disable this behavior: + +\code{.cpp} +MocoTropterSolver& solver = moco.initTropterSolver(); +solver.set_interpolate_control_midpoints(false); +\endcode + @section tips Tips for solving a custom problem 1. Make sure every variable has bounds. From 8526ebbb85e0359b6a7d02be936cffd9b4a36804 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 16 Sep 2024 17:05:22 -0700 Subject: [PATCH 38/38] Add FATROP tests --- .../MocoCasADiSolver/CasOCTranscription.cpp | 2 +- .../MocoCasADiSolver/CasOCTranscription.h | 14 +- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 28 ++++ OpenSim/Moco/Test/testMocoConstraints.cpp | 134 +++++++++++++++++- 4 files changed, 171 insertions(+), 7 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp index 027464be5d..8ead42003a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp @@ -1467,6 +1467,7 @@ void Transcription::printConstraintValues(const Iterate& it, ++ikc; } } + // TODO fix this to print kinematic values correctly ss << "Kinematic constraint values at each mesh point:" << std::endl; ss << " time "; @@ -1484,7 +1485,6 @@ void Transcription::printConstraintValues(const Iterate& it, ss << std::setprecision(2) << std::scientific << std::setw(9) << value << " "; } - // TODO fix this to print udoterr values correctly for (int iudot = 0; iudot < numUDotErr; ++iudot) { const auto& value = constraints.kinematic_udoterr(iudot, imesh).scalar(); diff --git a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h index 093072c858..3459b97de7 100644 --- a/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h +++ b/OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h @@ -509,11 +509,15 @@ class Transcription { } } - // Initial controls. + // Initial and final controls. if (imesh == 0) { copyColumn(constraints.initial_controls, 0); ng += constraints.initial_controls.rows(); } + if (imesh == m_numMeshIntervals - 1) { + copyColumn(constraints.final_controls, 0); + ng += constraints.final_controls.rows(); + } // Projection constraints. if (imesh > 0) { @@ -546,8 +550,6 @@ class Transcription { ng += path.rows(); } } - copyColumn(constraints.final_controls, 0); - ng += constraints.final_controls.rows(); copyColumn(constraints.projection, m_numMeshIntervals - 1); ng += constraints.projection.rows(); flat.ng.push_back(ng); @@ -646,10 +648,13 @@ class Transcription { } } - // Initial controls. + // Initial and final controls. if (imesh == 0) { copyColumn(out.initial_controls, 0); } + if (imesh == m_numMeshIntervals - 1) { + copyColumn(out.final_controls, 0); + } // Projection constraints. if (imesh > 0) { @@ -672,7 +677,6 @@ class Transcription { copyColumn(path, idyn); } } - copyColumn(out.final_controls, 0); copyColumn(out.projection, m_numMeshIntervals - 1); OPENSIM_THROW_IF(iflat != m_numConstraints, OpenSim::Exception, diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index c33b2a1f2a..b58c322676 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -278,6 +278,34 @@ std::unique_ptr MocoCasADiSolver::createCasOCSolver( } } + if (get_optim_solver() == "fatrop") { + if (get_optim_max_iterations() != -1) + solverOptions["max_iter"] = get_optim_max_iterations(); + + if (get_optim_convergence_tolerance() != -1) { + const auto& tol = get_optim_convergence_tolerance(); + solverOptions["tol"] = tol; + solverOptions["acceptable_tol"] = tol; + } + OPENSIM_THROW_IF_FRMOBJ(get_optim_constraint_tolerance() != -1, + Exception, + "The 'fatrop' solver does not utilize the constraint " + "tolerance."); + + OPENSIM_THROW_IF_FRMOBJ(get_optim_hessian_approximation() != "exact", + Exception, + "The 'fatrop' solver only supports the 'exact' hessian " + "approximation."); + + const auto& scheme = get_transcription_scheme(); + OPENSIM_THROW_IF_FRMOBJ(scheme == "trapezoidal" || + scheme == "hermite-simpson" || + scheme.find("radau") != std::string::npos, + Exception, + "The 'fatrop' solver only supports the 'legendre-gauss-#' " + "transcription schemes."); + } + checkPropertyValueIsInSet(getProperty_optim_sparsity_detection(), {"none", "random", "initial-guess"}); casSolver->setSparsityDetection(get_optim_sparsity_detection()); diff --git a/OpenSim/Moco/Test/testMocoConstraints.cpp b/OpenSim/Moco/Test/testMocoConstraints.cpp index 568a54fb9b..83f905258a 100644 --- a/OpenSim/Moco/Test/testMocoConstraints.cpp +++ b/OpenSim/Moco/Test/testMocoConstraints.cpp @@ -2221,6 +2221,7 @@ TEST_CASE("ConstantAccelerationConstraint") { MocoStudy study = createSlidingMassMocoStudy(model, scheme); MocoSolution solution = study.solve(); + const auto& states = solution.exportToStatesTrajectory(model); for (const auto& state : states) { model.realizeAcceleration(state); @@ -2230,6 +2231,137 @@ TEST_CASE("ConstantAccelerationConstraint") { // TODO enable test for Windows when MSVC build issues in FATROP are fixed. TEST_CASE("FATROP solver", "[casadi][unix]") { + + MocoStudy study; + study.setName("double_pendulum_study"); auto model = createDoublePendulumModel(); - + model->initSystem(); + + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(*model); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/jointset/j0/q0/value", {-5, 5}, 0); + mp.setStateInfo("/jointset/j0/q0/speed", {-10, 10}, 0, 0); + mp.setStateInfo("/jointset/j1/q1/value", {-10, 10}); + mp.setStateInfo("/jointset/j1/q1/speed", {-5, 5}, 0, 0); + mp.setControlInfo("/tau0", {-50, 50}); + mp.setControlInfo("/tau1", {-50, 50}); + auto* effort = mp.addGoal(); + + auto& ms = study.initCasADiSolver(); + ms.set_num_mesh_intervals(20); + ms.set_optim_solver("fatrop"); + ms.set_optim_hessian_approximation("exact"); + ms.set_optim_convergence_tolerance(1e-4); + ms.set_transcription_scheme("legendre-gauss-2"); + + SECTION("Disallowed solver settings") { + { + ms.set_optim_hessian_approximation("limited-memory"); + CHECK_THROWS_WITH(study.solve(), + Catch::Matchers::ContainsSubstring("The 'fatrop' solver only " + "supports the 'exact' hessian approximation.")); + ms.set_optim_hessian_approximation("exact"); + } + { + ms.set_optim_constraint_tolerance(1e-3); + CHECK_THROWS_WITH(study.solve(), + Catch::Matchers::ContainsSubstring("The 'fatrop' solver does " + "not utilize the constraint tolerance.")); + ms.set_optim_constraint_tolerance(-1); + } + { + ms.set_transcription_scheme("trapezoidal"); + CHECK_THROWS_WITH(study.solve(), + Catch::Matchers::ContainsSubstring("The 'fatrop' solver only " + "supports the 'legendre-gauss-#' transcription schemes.")); + } + { + ms.set_transcription_scheme("hermite-simpson"); + CHECK_THROWS_WITH(study.solve(), + Catch::Matchers::ContainsSubstring("The 'fatrop' solver only " + "supports the 'legendre-gauss-#' transcription schemes.")); + } + { + ms.set_transcription_scheme("legendre-gauss-radau-3"); + CHECK_THROWS_WITH(study.solve(), + Catch::Matchers::ContainsSubstring("The 'fatrop' solver only " + "supports the 'legendre-gauss-#' transcription schemes.")); + } + } + + SECTION("Basic problem") { + MocoSolution solution = study.solve(); + CHECK(solution.success()); + } + + // TODO produces "degenerate Jacobian" warning from FATROP + SECTION("Problem with kinematic constraints") { + const Coordinate& q0 = model->getCoordinateSet().get("q0"); + const Coordinate& q1 = model->getCoordinateSet().get("q1"); + CoordinateCouplerConstraint* constraint = + new CoordinateCouplerConstraint(); + Array indepCoordNames; + indepCoordNames.append("q0"); + constraint->setIndependentCoordinateNames(indepCoordNames); + constraint->setDependentCoordinateName("q1"); + + const SimTK::Real m = -2; + const SimTK::Real b = SimTK::Pi; + LinearFunction linFunc(m, b); + constraint->setFunction(&linFunc); + model->addConstraint(constraint); + model->finalizeConnections(); + + MocoProblem& mp = study.updProblem(); + mp.setModelAsCopy(*model); + + auto& ms = study.updSolver(); + ms.resetProblem(mp); + ms.set_kinematic_constraint_method("Bordalba2023"); + + MocoSolution solution = study.solve(); + CHECK(solution.success()); + } + + SECTION("Problem with path constraints") { + MocoProblem& mp = study.updProblem(); + auto* frameDistance = + mp.addPathConstraint(); + frameDistance->addFramePair("/bodyset/b0", "/bodyset/b1", 0.0, 5.0); + + auto& ms = study.updSolver(); + ms.resetProblem(mp); + + MocoSolution solution = study.solve(); + CHECK(solution.success()); + } + + SECTION("Problem with initial and final control bounds") { + MocoProblem& mp = study.updProblem(); + mp.setTimeBounds(0, 1); + mp.setStateInfo("/jointset/j0/q0/value", {-5, 5}); + mp.setStateInfo("/jointset/j0/q0/speed", {-10, 10}); + mp.setStateInfo("/jointset/j1/q1/value", {-10, 10}); + mp.setStateInfo("/jointset/j1/q1/speed", {-5, 5}); + mp.setControlInfo("/tau0", {-50, 50}, -4.3, 2.8); + mp.setControlInfo("/tau1", {-50, 50}, -3.2, 1.9); + + auto& ms = study.updSolver(); + ms.resetProblem(mp); + + MocoSolution solution = study.solve(); + CHECK(solution.success()); + + const auto& controls = solution.getControlsTrajectory(); + int N = solution.getNumTimes(); + CHECK_THAT(controls(0, 0), Catch::Matchers::WithinAbs(-4.3, 1e-6)); + CHECK_THAT(controls(0, 1), Catch::Matchers::WithinAbs(-3.2, 1e-6)); + CHECK_THAT(controls(N-1, 0), Catch::Matchers::WithinAbs(2.8, 1e-6)); + CHECK_THAT(controls(N-1, 1), Catch::Matchers::WithinAbs(1.9, 1e-6)); + } + + SECTION("Endpoint constraints are disallowed") { + // TODO + } }