diff --git a/CHANGELOG_MOCO.md b/CHANGELOG_MOCO.md index 0ada0937e8..cc9d859747 100644 --- a/CHANGELOG_MOCO.md +++ b/CHANGELOG_MOCO.md @@ -1,8 +1,11 @@ Moco Change Log =============== -1.2.2 +1.3.0 ----- +- 2023-11-08: Fixed a bug where `MocoTrajectory::generateSpeedsFromValues()` was + accidentally deleting auxiliary state values. + - 2023-10-25: Fixed a bug preventing deserialization of `MocoFrameDistanceConstraint`. - 2023-10-25: Locked coordinates are now explicitly disallowed in `MocoProblem`s, diff --git a/OpenSim/Moco/MocoTrajectory.cpp b/OpenSim/Moco/MocoTrajectory.cpp index 75684af0b6..128ce6773d 100644 --- a/OpenSim/Moco/MocoTrajectory.cpp +++ b/OpenSim/Moco/MocoTrajectory.cpp @@ -369,17 +369,24 @@ void MocoTrajectory::insertControlsTrajectory( void MocoTrajectory::generateSpeedsFromValues() { auto valuesTable = exportToValuesTable(); int numValues = (int)valuesTable.getNumColumns(); + const std::vector& valueNames = valuesTable.getColumnLabels(); OPENSIM_THROW_IF(!numValues, Exception, "Tried to compute speeds from coordinate values, but no values " "exist in the trajectory."); - m_states.resize(getNumTimes(), 2*numValues); + + // Save the auxiliary states to fill back in later. + SimTK::Matrix auxiliaryStates = getAuxiliaryStatesTrajectory(); + std::vector auxiliaryStateNames = getAuxiliaryStateNames(); + + // Resize the states trajectory to hold the values, speeds, and any + // auxiliary states. + m_states.resize(getNumTimes(), 2*numValues + getNumAuxiliaryStates()); // Spline the values trajectory. GCVSplineSet splines(valuesTable, {}, std::min(getNumTimes() - 1, 5)); + // Create the speed names. std::vector stateNames; - const std::vector& valueNames = - valuesTable.getColumnLabels(); std::vector speedNames; for (int ivalue = 0; ivalue < numValues; ++ivalue) { std::string name(valueNames[ivalue]); @@ -389,6 +396,14 @@ void MocoTrajectory::generateSpeedsFromValues() { speedNames.push_back(name); } + // Append the speed names to the state names. + stateNames.insert(stateNames.end(), speedNames.begin(), speedNames.end()); + + // Append the auxiliary state names to the state names. + stateNames.insert(stateNames.end(), auxiliaryStateNames.begin(), + auxiliaryStateNames.end()); + + // Fill in the coordinate and speed values based on the splined values. SimTK::Vector currTime(1, SimTK::NaN); for (int ivalue = 0; ivalue < numValues; ++ivalue) { const auto& name = valueNames[ivalue]; @@ -402,6 +417,14 @@ void MocoTrajectory::generateSpeedsFromValues() { } } + // Fill back in any auxiliary state values. + for (int iaux = 0; iaux < getNumAuxiliaryStates(); ++iaux) { + for (int itime = 0; itime < m_time.size(); ++itime) { + currTime[0] = m_time[itime]; + m_states(itime, 2*numValues + iaux) = auxiliaryStates(itime, iaux); + } + } + // Assign state names. m_state_names = stateNames; } diff --git a/OpenSim/Moco/MocoTrajectory.h b/OpenSim/Moco/MocoTrajectory.h index 76891f0f41..f926538762 100644 --- a/OpenSim/Moco/MocoTrajectory.h +++ b/OpenSim/Moco/MocoTrajectory.h @@ -472,9 +472,20 @@ class OSIMMOCO_API MocoTrajectory { } int getNumSpeeds() const { + ensureUnsealed(); return (int)getSpeedIndices().size(); } + int getNumMultibodyStates() const { + ensureUnsealed(); + return (int)getMultibodyStateIndices().size(); + } + + int getNumAuxiliaryStates() const { + ensureUnsealed(); + return (int)getAuxiliaryStateIndices().size(); + } + int getNumAccelerations() const { ensureUnsealed(); return (int)getAccelerationIndices().size(); @@ -528,6 +539,29 @@ class OSIMMOCO_API MocoTrajectory { } return speedNames; } + std::vector getMultibodyStateNames() const { + ensureUnsealed(); + std::vector multibodyStateNames; + for (const auto& name : m_state_names) { + if (name.find("/value") != std::string::npos) { + multibodyStateNames.push_back(name); + } else if (name.find("/speed") != std::string::npos) { + multibodyStateNames.push_back(name); + } + } + return multibodyStateNames; + } + std::vector getAuxiliaryStateNames() const { + ensureUnsealed(); + std::vector auxiliaryStateNames; + for (const auto& name : m_state_names) { + if (name.find("/value") == std::string::npos && + name.find("/speed") == std::string::npos) { + auxiliaryStateNames.push_back(name); + } + } + return auxiliaryStateNames; + } std::vector getAccelerationNames() const { ensureUnsealed(); std::vector accelerationNames; @@ -589,6 +623,20 @@ class OSIMMOCO_API MocoTrajectory { return {m_states.block(0, indices[0], m_states.nrow(), getNumSpeeds())}; } + SimTK::Matrix getMultibodyStatesTrajectory() const { + ensureUnsealed(); + auto indices = getMultibodyStateIndices(); + if (indices.empty()) indices.push_back(0); + return {m_states.block(0, indices[0], + m_states.nrow(), getNumMultibodyStates())}; + } + SimTK::Matrix getAuxiliaryStatesTrajectory() const { + ensureUnsealed(); + auto indices = getAuxiliaryStateIndices(); + if (indices.empty()) indices.push_back(0); + return {m_states.block(0, indices[0], + m_states.nrow(), getNumAuxiliaryStates())}; + } SimTK::Matrix getAccelerationsTrajectory() const { ensureUnsealed(); auto indices = getAccelerationIndices(); @@ -877,6 +925,31 @@ class OSIMMOCO_API MocoTrajectory { } return speedIndices; } + std::vector getMultibodyStateIndices() const { + ensureUnsealed(); + std::vector multibodyStateIndices; + auto valueIndices = getValueIndices(); + auto speedIndices = getSpeedIndices(); + multibodyStateIndices.reserve( + valueIndices.size() + speedIndices.size()); + multibodyStateIndices.insert(multibodyStateIndices.end(), + valueIndices.begin(), valueIndices.end()); + multibodyStateIndices.insert(multibodyStateIndices.end(), + speedIndices.begin(), speedIndices.end()); + return multibodyStateIndices; + } + std::vector getAuxiliaryStateIndices() const { + ensureUnsealed(); + std::vector auxiliaryStateIndices; + for (int i = 0; i < (int)m_state_names.size(); ++i) { + const auto& name = m_state_names[i]; + if (name.find("/value") == std::string::npos && + name.find("/speed") == std::string::npos) { + auxiliaryStateIndices.push_back(i); + } + } + return auxiliaryStateIndices; + } std::vector getAccelerationIndices() const { ensureUnsealed(); std::vector accelerationIndices; diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index ed5d07b7e3..11570f47fd 100644 --- a/OpenSim/Moco/Test/testMocoInterface.cpp +++ b/OpenSim/Moco/Test/testMocoInterface.cpp @@ -2126,8 +2126,41 @@ TEST_CASE("Objective breakdown", "[casadi]") { CHECK(solution.getObjectiveTerm("goal_b") == Approx(0.01 * 7.3)); } +TEST_CASE("generateSpeedsFromValues() does not overwrite auxiliary states.") { + int N = 20; + SimTK::Vector time = createVectorLinspace(20, 0.0, 1.0); + std::vector snames{"/jointset/joint/coord/value", + "/jointset/joint/coord/speed", + "/forceset/muscle/normalized_tendon_force"}; + std::vector cnames{"/forceset/muscle"}; + std::vector dnames{ + "/forceset/muscle/implicitderiv_normalized_tendon_force"}; + SimTK::Matrix states = SimTK::Test::randMatrix(N, 3); + SimTK::Matrix controls = SimTK::Test::randMatrix(N, 1); + SimTK::Matrix derivatives = SimTK::Test::randMatrix(N, 1); + MocoTrajectory traj(time, snames, cnames, {}, dnames, {}, states, controls, + SimTK::Matrix(), derivatives, SimTK::RowVector()); + + traj.generateSpeedsFromValues(); + CHECK(traj.getNumStates() == 3); + CHECK(traj.getStateNames() == snames); + CHECK(traj.getNumDerivatives() == 1); + CHECK(traj.getDerivativeNames() == dnames); + CHECK(traj.getNumAuxiliaryStates() == 1); + std::vector + auxnames{"/forceset/muscle/normalized_tendon_force"}; + CHECK(traj.getAuxiliaryStateNames() == auxnames); + + SimTK::Matrix auxiliaryStates = traj.getAuxiliaryStatesTrajectory(); + double error = 0.0; + for (int irow = 0; irow < states.nrow(); ++irow) { + error += pow(states(irow, 2) - auxiliaryStates(irow, 0), 2); + } + SimTK_TEST_EQ(error, 0.0); +} + TEST_CASE("generateAccelerationsFromXXX() does not overwrite existing " - "non-accleration derivatives.") { + "non-acceleration derivatives.") { int N = 20; SimTK::Vector time = createVectorLinspace(20, 0.0, 1.0); std::vector snames{"/jointset/joint/coord/value",