Update MocoTrajectory::generateSpeedsFromValues() to not override a…

…uxiliary states (#3603)

* Fix MocoTrajectory::generateSpeedsFromValues() so it does not override auxiliary states

* Update Moco changelog

* Merge branch 'main' into moco_trajectory_generate_fix

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, 

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<std::string>& 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<std::string> 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<std::string> stateNames;
-    const std::vector<std::string>& valueNames =
-            valuesTable.getColumnLabels();
     std::vector<std::string> 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;
 }

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<std::string> getMultibodyStateNames() const {
+        ensureUnsealed();
+        std::vector<std::string> 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<std::string> getAuxiliaryStateNames() const {
+        ensureUnsealed();
+        std::vector<std::string> 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<std::string> getAccelerationNames() const {
         ensureUnsealed();
         std::vector<std::string> 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<int> getMultibodyStateIndices() const {
+        ensureUnsealed();
+        std::vector<int> 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<int> getAuxiliaryStateIndices() const {
+        ensureUnsealed();
+        std::vector<int> 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<int> getAccelerationIndices() const {
         ensureUnsealed();
         std::vector<int> accelerationIndices;

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<std::string> snames{"/jointset/joint/coord/value",
+                                    "/jointset/joint/coord/speed",
+                                    "/forceset/muscle/normalized_tendon_force"};
+    std::vector<std::string> cnames{"/forceset/muscle"};
+    std::vector<std::string> 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<std::string>
+            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<std::string> snames{"/jointset/joint/coord/value",