{"action":"created","comment":{"url":"https://api.github.com/repos/opensim-org/opensim-core/pulls/comments/52135380","id":52135380,"diff_hunk":"@@ -0,0 +1,877 @@\n+/* -------------------------------------------------------------------------- *\n+ *                     OpenSim:  testStatesTrajectory.cpp                     *\n+ * -------------------------------------------------------------------------- *\n+ * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *\n+ * See http://opensim.stanford.edu and the NOTICE file for more information.  *\n+ * OpenSim is developed at Stanford University and supported by the US        *\n+ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *\n+ * through the Warrior Web program.                                           *\n+ *                                                                            *\n+ * Copyright (c) 2005-2016 Stanford University and the Authors                *\n+ * Author(s): Chris Dembia                                                    *\n+ *                                                                            *\n+ * Licensed under the Apache License, Version 2.0 (the \"License\"); you may    *\n+ * not use this file except in compliance with the License. You may obtain a  *\n+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *\n+ *                                                                            *\n+ * Unless required by applicable law or agreed to in writing, software        *\n+ * distributed under the License is distributed on an \"AS IS\" BASIS,          *\n+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *\n+ * See the License for the specific language governing permissions and        *\n+ * limitations under the License.                                             *\n+ * -------------------------------------------------------------------------- */\n+\n+#include <OpenSim/Simulation/osimSimulation.h>\n+#include <OpenSim/Common/Constant.h>\n+#include <OpenSim/Common/LoadOpenSimLibrary.h>\n+#include <random>\n+#include <cstdio>\n+\n+using namespace OpenSim;\n+using namespace SimTK;\n+\n+// TODO detailed exceptions when integrity checks fail.\n+// TODO currently, one gets segfaults if state is not realized.\n+// TODO accessing acceleration-level outputs.\n+\n+// TODO append two StateTrajectories together.\n+// TODO test modeling options (locked coordinates, etc.)\n+// TODO store a model within a StatesTrajectory.\n+\n+const std::string statesStoFname = \"testStatesTrajectory_readStorage_states.sto\";\n+const std::string pre40StoFname = \"std_subject01_walk1_states.sto\";\n+\n+// Helper function to get a state variable value from a storage file.\n+Real getStorageEntry(const Storage& sto,\n+        const int timeIndex, const std::string& columnName) {\n+    Real value;\n+    const int columnIndex = sto.getStateIndex(columnName);\n+    sto.getData(timeIndex, columnIndex, value);\n+    return value;\n+}\n+\n+void testPopulateTrajectory() {\n+    Model model(\"gait2354_simbody.osim\");\n+\n+    // To assist with creating interesting (non-zero) coordinate values:\n+    model.updCoordinateSet().get(\"pelvis_ty\").setDefaultLocked(true);\n+\n+    auto& state = model.initSystem();\n+\n+    SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem());\n+    SimTK::TimeStepper ts(model.getSystem(), integrator);\n+    ts.initialize(state);\n+    ts.setReportAllSignificantStates(true);\n+    integrator.setReturnEveryInternalStep(true);\n+\n+    StatesTrajectory states;\n+    const double finalTime = 0.05;\n+    std::vector<double> times;\n+    while (ts.getState().getTime() < finalTime) {\n+        ts.stepTo(finalTime);\n+        times.push_back(ts.getState().getTime());\n+        // StatesTrajectory API for appending states:\n+        states.append(ts.getState());\n+    }\n+\n+    // Make sure we have all the states\n+    SimTK_TEST_EQ((int)states.getSize(), (int)times.size());\n+    // ...and that they aren't all just references to the same single state.\n+    for (int i = 0; i < states.getSize(); ++i) {\n+        SimTK_TEST_EQ(states[i].getTime(), times[i]);\n+    }\n+\n+}\n+\n+void testFrontBack() {\n+    Model model(\"arm26.osim\");\n+    const auto& state = model.initSystem();\n+    StatesTrajectory states;\n+    states.append(state);\n+    states.append(state);\n+    states.append(state);\n+\n+    SimTK_TEST(&states.front() == &states[0]);\n+    SimTK_TEST(&states.back() == &states[2]);\n+}\n+\n+// Create states storage file to for states storage tests.\n+void createStateStorageFile() {\n+\n+    Model model(\"gait2354_simbody.osim\");\n+\n+    // To assist with creating interesting (non-zero) coordinate values:\n+    model.updCoordinateSet().get(\"pelvis_ty\").setDefaultLocked(true);\n+\n+    // Randomly assign muscle excitations to create interesting activation\n+    // histories.\n+    auto* controller = new PrescribedController();\n+    // For consistent results, use same seed each time.\n+    std::default_random_engine generator(0); \n+    // Uniform distribution between 0.1 and 0.9.\n+    std::uniform_real_distribution<double> distribution(0.1, 0.8);\n+\n+    for (int im = 0; im < model.getMuscles().getSize(); ++im) {\n+        controller->addActuator(model.getMuscles()[im]);\n+        controller->prescribeControlForActuator(\n+            model.getMuscles()[im].getName(),\n+            new Constant(distribution(generator))\n+            );\n+    }\n+\n+    model.addController(controller);\n+\n+    auto& initState = model.initSystem();\n+    SimTK::RungeKuttaMersonIntegrator integrator(model.getSystem());\n+    Manager manager(model, integrator);\n+    manager.setFinalTime(0.15);\n+    manager.integrate(initState);\n+    manager.getStateStorage().print(statesStoFname);\n+}\n+\n+void testFromStatesStorageGivesCorrectStates() {\n+\n+    // Read in trajectory.\n+    // -------------------\n+    Model model(\"gait2354_simbody.osim\");\n+\n+    Storage sto(statesStoFname);\n+\n+    // It's important that we have not yet initialized the model, \n+    // since `createFromStatesStorage()` should be able to work with such a\n+    // model.\n+    auto states = StatesTrajectory::createFromStatesStorage(model, sto);\n+\n+    // However, we eventually *do* need to call initSystem() to make use of the\n+    // trajectory with the model.\n+    model.initSystem();\n+\n+    // Test that the states are correct, and also that the iterator works.\n+    // -------------------------------------------------------------------\n+    int itime = 0;\n+    double currTime;\n+    for (const auto& state : states) {\n+        // Time.\n+        sto.getTime(itime, currTime);\n+        SimTK_TEST_EQ(currTime, state.getTime());\n+\n+        // Multibody states.\n+        for (int ic = 0; ic < model.getCoordinateSet().getSize(); ++ic) {\n+            const auto& coord = model.getCoordinateSet().get(ic);\n+            auto coordName = coord.getName();\n+            auto jointName = coord.getJoint().getName();\n+            auto coordPath = jointName + \"/\" + coordName;\n+            // Coordinate.\n+            SimTK_TEST_EQ(getStorageEntry(sto, itime, coordPath + \"/value\"),\n+                    coord.getValue(state));\n+\n+            // Speed.\n+            SimTK_TEST_EQ(getStorageEntry(sto, itime, coordPath + \"/speed\"),\n+                    coord.getSpeedValue(state));\n+        }\n+\n+        // Muscle states.\n+        for (int im = 0; im < model.getMuscles().getSize(); ++im) {\n+            const auto& muscle = model.getMuscles().get(im);\n+            auto muscleName = muscle.getName();\n+\n+            // Activation.\n+            {\n+                std::string stateVarName = muscleName + \"/activation\";\n+                SimTK_TEST_EQ(\n+                        getStorageEntry(sto, itime, stateVarName),\n+                        muscle.getStateVariableValue(state, stateVarName));\n+            }\n+\n+            // Fiber length.\n+            {\n+                std::string stateVarName = muscleName + \"/fiber_length\";\n+                SimTK_TEST_EQ(\n+                        getStorageEntry(sto, itime, stateVarName), \n+                        muscle.getStateVariableValue(state, stateVarName));\n+            }\n+\n+        }\n+\n+        // More complicated computation based on state.\n+        // These calculations require that the state is realized to velocity.\n+        model.getMultibodySystem().realize(state, SimTK::Stage::Velocity);\n+\n+        for (int im = 0; im < model.getMuscles().getSize(); ++im) {\n+            const auto& muscle = model.getMuscles().get(im);\n+            auto muscleName = muscle.getName();\n+            SimTK_TEST(!SimTK::isNaN(muscle.getFiberForce(state)));\n+        }\n+\n+        auto loc = model.getBodySet().get(\"tibia_r\")\n+            .findLocationInAnotherFrame(state,\n+                    SimTK::Vec3(1, 0.5, 0.25),\n+                    model.getGround());\n+        SimTK_TEST(!loc.isNaN());\n+\n+        SimTK_TEST(!model.calcMassCenterVelocity(state).isNaN());\n+\n+        // TODO acceleration-level stuff gives segfault.\n+        // TODO model.getMultibodySystem().realize(state, SimTK::Stage::Acceleration);\n+        // TODO SimTK_TEST(!model.calcMassCenterAcceleration(state).isNaN());\n+\n+        itime++;\n+    }\n+}\n+\n+Storage newStorageWithRemovedRows(const Storage& origSto,\n+        const std::set<int>& rowsToRemove) {\n+    Storage sto(1000);\n+    auto labels = origSto.getColumnLabels();\n+    auto numOrigColumns = origSto.getColumnLabels().getSize() - 1;\n+\n+    // Remove in reverse order so it's easier to keep track of indices.\n+     for (auto it = rowsToRemove.rbegin(); it != rowsToRemove.rend(); ++it) {\n+         labels.remove(*it);\n+     }\n+    sto.setColumnLabels(labels);\n+\n+    double time;\n+    for (int itime = 0; itime < origSto.getSize(); ++itime) {\n+        SimTK::Vector rowData(numOrigColumns);\n+        origSto.getData(itime, numOrigColumns, rowData);\n+\n+        SimTK::Vector newRowData(numOrigColumns - (int)rowsToRemove.size());\n+        int iNew = 0;\n+        for (int iOrig = 0; iOrig < numOrigColumns; ++iOrig) {\n+            if (rowsToRemove.count(iOrig) == 0) {\n+                newRowData[iNew] = rowData[iOrig];\n+                ++iNew;\n+            }\n+        }\n+\n+        origSto.getTime(itime, time);\n+        sto.append(time, newRowData);\n+    }\n+    return sto;\n+}\n+\n+void testFromStatesStorageInconsistentModel(const std::string &stoFilepath) {\n+\n+    // States are missing from the Storage.\n+    // ------------------------------------\n+    {\n+        Model model(\"gait2354_simbody.osim\");\n+        // Needed for getting state variable names.\n+        model.initSystem();\n+\n+        const auto stateNames = model.getStateVariableNames();\n+        Storage sto(stoFilepath);\n+        // So the test doesn't take so long.\n+        sto.resampleLinear((sto.getLastTime() - sto.getFirstTime()) / 10);\n+\n+        // Create new Storage with fewer columns.\n+        auto labels = sto.getColumnLabels();\n+\n+        auto origLabel10 = stateNames[10];\n+        auto origLabel15 = stateNames[15];\n+        Storage stoMissingCols = newStorageWithRemovedRows(sto, {\n+                // gymnastics to be compatible with pre-v4.0 column names:\n+                sto.getStateIndex(origLabel10) + 1,\n+                sto.getStateIndex(origLabel15) + 1});\n+\n+        // Test that an exception is thrown.\n+        SimTK_TEST_MUST_THROW_EXC(\n+                StatesTrajectory::createFromStatesStorage(model, stoMissingCols),\n+                StatesTrajectory::MissingColumnsInStatesStorage\n+                );\n+        // Check some other similar calls.\n+        SimTK_TEST_MUST_THROW_EXC(\n+                StatesTrajectory::createFromStatesStorage(model, stoMissingCols,\n+                    false, true),\n+                StatesTrajectory::MissingColumnsInStatesStorage\n+                );\n+        SimTK_TEST_MUST_THROW_EXC(\n+                StatesTrajectory::createFromStatesStorage(model, stoMissingCols,\n+                    false, false),\n+                StatesTrajectory::MissingColumnsInStatesStorage\n+                );\n+\n+        // No exception if allowing missing columns.\n+        // The unspecified states are set to NaN (for at least two random\n+        // states).\n+        auto states = StatesTrajectory::createFromStatesStorage(\n+                model, stoMissingCols, true);\n+        SimTK_TEST(SimTK::isNaN(\n+                    model.getStateVariableValue(states[0], origLabel10)));\n+        SimTK_TEST(SimTK::isNaN(\n+                    model.getStateVariableValue(states[4], origLabel15)));\n+        // Behavior is independent of value for allowMissingColumns.\n+        StatesTrajectory::createFromStatesStorage(model, stoMissingCols,\n+                true, true);\n+        StatesTrajectory::createFromStatesStorage(model, stoMissingCols,\n+                true, false);\n+    }\n+\n+    // States are missing from the Model.\n+    // ----------------------------------\n+    {\n+        Model model(\"gait2354_simbody.osim\");\n+        Storage sto(stoFilepath);\n+        // So the test doesn't take so long.\n+        sto.resampleLinear((sto.getLastTime() - sto.getFirstTime()) / 10);\n+\n+        // Remove a few of the muscles.\n+        model.updForceSet().remove(0);\n+        model.updForceSet().remove(10);\n+        model.updForceSet().remove(30);\n+\n+        // Test that an exception is thrown.\n+        SimTK_TEST_MUST_THROW_EXC(\n+                StatesTrajectory::createFromStatesStorage(model, sto),\n+                StatesTrajectory::ExtraColumnsInStatesStorage\n+                );\n+        SimTK_TEST_MUST_THROW_EXC(\n+                StatesTrajectory::createFromStatesStorage(model, sto,\n+                    true, false),\n+                StatesTrajectory::ExtraColumnsInStatesStorage\n+                );\n+        SimTK_TEST_MUST_THROW_EXC(\n+                StatesTrajectory::createFromStatesStorage(model, sto,\n+                    false, false),\n+                StatesTrajectory::ExtraColumnsInStatesStorage\n+                );\n+\n+        // No exception if allowing extra columns, and behavior is\n+        // independent of value for allowMissingColumns.\n+        StatesTrajectory::createFromStatesStorage(model, sto, true, true);\n+        StatesTrajectory::createFromStatesStorage(model, sto, false, true);\n+    }\n+}\n+\n+void testFromStatesStorageUniqueColumnLabels() {\n+\n+    Model model(\"gait2354_simbody.osim\");\n+    Storage sto(statesStoFname);\n+    \n+    // Edit column labels so that they are not unique.\n+    auto labels = sto.getColumnLabels();\n+    labels[10] = labels[7];\n+    sto.setColumnLabels(labels); \n+   \n+    SimTK_TEST_MUST_THROW_EXC(\n+            StatesTrajectory::createFromStatesStorage(model, sto),\n+            StatesTrajectory::NonUniqueColumnsInStatesStorage);\n+    SimTK_TEST_MUST_THROW_EXC(\n+            StatesTrajectory::createFromStatesStorage(model, sto, true, true),\n+            StatesTrajectory::NonUniqueColumnsInStatesStorage);\n+    SimTK_TEST_MUST_THROW_EXC(\n+            StatesTrajectory::createFromStatesStorage(model, sto, true, false),\n+            StatesTrajectory::NonUniqueColumnsInStatesStorage);\n+    SimTK_TEST_MUST_THROW_EXC(\n+            StatesTrajectory::createFromStatesStorage(model, sto, false, true),\n+            StatesTrajectory::NonUniqueColumnsInStatesStorage);\n+    SimTK_TEST_MUST_THROW_EXC(\n+            StatesTrajectory::createFromStatesStorage(model, sto, false, false),\n+            StatesTrajectory::NonUniqueColumnsInStatesStorage);\n+\n+    // TODO unique even considering old and new formats for state variable\n+    // names (/value and /speed) in the same file.\n+}\n+\n+void testFromStatesStoragePre40CorrectStates() {\n+    // This test is very similar to testFromStatesStorageGivesCorrectStates\n+    // TODO could avoid the duplicate function since getStateIndex handles\n+    // pre-4.0 names.\n+\n+    // Read in trajectory.\n+    // -------------------\n+    Model model(\"gait2354_simbody.osim\");\n+\n+    Storage sto(pre40StoFname);\n+    // So the test doesn't take so long.\n+    sto.resampleLinear(0.01);\n+    auto states = StatesTrajectory::createFromStatesStorage(model, sto);\n+\n+    model.initSystem();\n+\n+    // Test that the states are correct.\n+    // ---------------------------------\n+    int itime = 0;\n+    double currTime;\n+    for (const auto& state : states) {\n+        // Time.\n+        sto.getTime(itime, currTime);\n+        SimTK_TEST_EQ(currTime, state.getTime());\n+\n+        // Multibody states.\n+        for (int ic = 0; ic < model.getCoordinateSet().getSize(); ++ic) {\n+            const auto& coord = model.getCoordinateSet().get(ic);\n+            auto coordName = coord.getName();\n+\n+            // Coordinate.\n+            SimTK_TEST_EQ(getStorageEntry(sto, itime, coordName),\n+                    coord.getValue(state));\n+\n+            // Speed.\n+            SimTK_TEST_EQ(getStorageEntry(sto, itime, coordName + \"_u\"),\n+                    coord.getSpeedValue(state));\n+        }\n+\n+        // Muscle states.\n+        for (int im = 0; im < model.getMuscles().getSize(); ++im) {\n+            const auto& muscle = model.getMuscles().get(im);\n+            auto muscleName = muscle.getName();\n+\n+            // Activation.\n+            // TODO Simply accessing these state variables requires realizing\n+            // to Velocity; I think this is a bug.\n+            model.getMultibodySystem().realize(state, SimTK::Stage::Velocity);\n+            SimTK_TEST_EQ(\n+                    getStorageEntry(sto, itime, muscleName + \".activation\"),\n+                    muscle.getActivation(state));\n+\n+            // Fiber length.\n+            SimTK_TEST_EQ(\n+                    getStorageEntry(sto, itime, muscleName + \".fiber_length\"), \n+                    muscle.getFiberLength(state));\n+\n+            // More complicated computation based on state.\n+            SimTK_TEST(!SimTK::isNaN(muscle.getFiberForce(state)));\n+        }\n+\n+        // More complicated computations based on state.\n+        auto loc = model.getBodySet().get(\"tibia_r\")\n+            .findLocationInAnotherFrame(state,\n+                    SimTK::Vec3(1, 0.5, 0.25),\n+                    model.getGround());\n+        SimTK_TEST(!loc.isNaN());\n+\n+        SimTK_TEST(!model.calcMassCenterVelocity(state).isNaN());\n+        // TODO acceleration-level stuff gives segfault.\n+        // TODO model.getMultibodySystem().realize(state, SimTK::Stage::Acceleration);\n+        // TODO SimTK_TEST(!model.calcMassCenterAcceleration(state).isNaN());\n+\n+        itime++;\n+    }\n+}\n+\n+void testFromStatesStorageAllRowsHaveSameLength() {\n+    // Missing data in individual rows leads to an incorrect StatesTrajectory\n+    // and could really confuse users.\n+    Model model(\"gait2354_simbody.osim\");\n+    model.initSystem();\n+\n+    const auto stateNames = model.getStateVariableNames();\n+    Storage sto(statesStoFname);\n+    // Append a too-short state vector.\n+    std::vector<double> v(model.getNumStateVariables() - 10, 1.0);\n+    StateVector sv(25.0, model.getNumStateVariables() - 10, v.data());\n+    sto.append(sv);\n+\n+    SimTK_TEST_MUST_THROW_EXC(\n+            StatesTrajectory::createFromStatesStorage(model, sto),\n+            StatesTrajectory::VaryingNumberOfStatesPerRow);\n+}\n+\n+\n+void testCopying() {\n+    Model model(\"gait2354_simbody.osim\");\n+    auto& state = model.initSystem();\n+\n+    StatesTrajectory states;\n+    {\n+        state.setTime(0.5);\n+        states.append(state);\n+        state.setTime(1.3);\n+        states.append(state);\n+        state.setTime(3.5);\n+        states.append(state);\n+    }\n+\n+    {\n+        StatesTrajectory statesCopyConstruct(states);\n+        // Ideally we'd check for equality (operator==()), but State does not\n+        // have an equality operator.\n+        SimTK_TEST_EQ((int)statesCopyConstruct.getSize(), (int)states.getSize());\n+        for (int i = 0; i < states.getSize(); ++i) {\n+            SimTK_TEST_EQ(statesCopyConstruct[i].getTime(), states[i].getTime());\n+        }\n+    }\n+\n+    {\n+        StatesTrajectory statesCopyAssign;\n+        statesCopyAssign = states;\n+        SimTK_TEST_EQ((int)statesCopyAssign.getSize(), (int)states.getSize());\n+        for (int i = 0; i < states.getSize(); ++i) {\n+            SimTK_TEST_EQ(statesCopyAssign[i].getTime(), states[i].getTime());\n+        }\n+    }\n+}\n+\n+void testAccessByTime() {\n+    Model model(\"arm26.osim\");\n+    auto& state = model.initSystem();\n+\n+    // Make a trajectory with different times.\n+    StatesTrajectory states;\n+    state.setTime(0.5);\n+    states.append(state); // 0\n+    state.setTime(1.3);\n+    states.append(state); // 1\n+    state.setTime(3.5);\n+    states.append(state); // 2\n+    state.setTime(4.1);\n+    states.append(state); // 3\n+    \n+    // Nearest before.\n+    SimTK_TEST(states.getIndexBefore(0.5) == 0);\n+    SimTK_TEST(states.getIndexBefore(0.5000001) == 0);\n+    SimTK_TEST(states.getIndexBefore(1.299999999) == 0);\n+\n+    SimTK_TEST(states.getIndexBefore(1.3) == 1);\n+    SimTK_TEST(states.getIndexBefore(1.300001) == 1);\n+    SimTK_TEST(states.getIndexBefore(3.49999) == 1);\n+\n+    SimTK_TEST(states.getIndexBefore(4.1) == 3);\n+    SimTK_TEST(states.getIndexBefore(4.2) == 3);\n+    SimTK_TEST(states.getIndexBefore(153.7) == 3);\n+\n+    // Nearest after.\n+    SimTK_TEST(states.getIndexAfter(-100.0) == 0);\n+    SimTK_TEST(states.getIndexAfter(0) == 0);\n+    SimTK_TEST(states.getIndexAfter(0.1) == 0);\n+    SimTK_TEST(states.getIndexAfter(0.4999999999) == 0);\n+    SimTK_TEST(states.getIndexAfter(0.5) == 0);\n+\n+    SimTK_TEST(states.getIndexAfter(1.3000000000001) == 2);\n+    SimTK_TEST(states.getIndexAfter(1.31) == 2);\n+    SimTK_TEST(states.getIndexAfter(1.4) == 2);\n+    SimTK_TEST(states.getIndexAfter(3.4) == 2);\n+    SimTK_TEST(states.getIndexAfter(3.4999999999999) == 2);\n+    SimTK_TEST(states.getIndexAfter(3.5) == 2);\n+    \n+    SimTK_TEST(states.getIndexAfter(4.0001) == 3);\n+    SimTK_TEST(states.getIndexAfter(4.1) == 3);\n+\n+    // Exception if there is not a state before/after the specified time.\n+    SimTK_TEST_MUST_THROW_EXC(states.getIndexBefore(0.25),\n+            StatesTrajectory::TimeOutOfRange);\n+    SimTK_TEST_MUST_THROW_EXC(states.getIndexBefore(0.0),\n+            StatesTrajectory::TimeOutOfRange);\n+    SimTK_TEST_MUST_THROW_EXC(states.getIndexBefore(-1.0),\n+            StatesTrajectory::TimeOutOfRange);\n+    SimTK_TEST_MUST_THROW_EXC(states.getIndexBefore(.499999),\n+            StatesTrajectory::TimeOutOfRange);\n+\n+    SimTK_TEST_MUST_THROW_EXC(states.getIndexAfter(4.10000001),\n+            StatesTrajectory::TimeOutOfRange);\n+    SimTK_TEST_MUST_THROW_EXC(states.getIndexAfter(1050.9),\n+            StatesTrajectory::TimeOutOfRange);\n+\n+    // Simulate the scenario that the tolerance is intended to handle.\n+    {\n+        // Pretend that the original states had times of 4.0 and 5.0, but\n+        // that they were serialized (to a storage file) with limited precision\n+        // and were written as 3.999999 and 5.000001. Now, the user\n+        // thinks these times are still 4.0, and 5.0, and will end up\n+        // getting the wrong states.\n+        StatesTrajectory states2;\n+        state.setTime(1.9);\n+        states2.append(state); // 0\n+        state.setTime(3.999999);\n+        states2.append(state); // 1\n+        state.setTime(4.5);\n+        states2.append(state); // 2\n+        state.setTime(5.000001);\n+        states2.append(state); // 3\n+        state.setTime(9.3);\n+        states2.append(state); // 4\n+        SimTK_TEST(states2.getIndexAfter(4.0) != 1);\n+        SimTK_TEST(states2.getIndexBefore(5.0) != 3);","path":"OpenSim/Simulation/Test/testStatesTrajectory.cpp","position":null,"original_position":586,"commit_id":"7ffd6179556c79d4491c072f06cd568c7dafc201","original_commit_id":"065f7af2236319b98d4a830f7a1004637d0bd859","user":{"login":"chrisdembia","id":846001,"avatar_url":"https://avatars.githubusercontent.com/u/846001?v=3","gravatar_id":"","url":"https://api.github.com/users/chrisdembia","html_url":"https://github.com/chrisdembia","followers_url":"https://api.github.com/users/chrisdembia/followers","following_url":"https://api.github.com/users/chrisdembia/following{/other_user}","gists_url":"https://api.github.com/users/chrisdembia/gists{/gist_id}","starred_url":"https://api.github.com/users/chrisdembia/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/chrisdembia/subscriptions","organizations_url":"https://api.github.com/users/chrisdembia/orgs","repos_url":"https://api.github.com/users/chrisdembia/repos","events_url":"https://api.github.com/users/chrisdembia/events{/privacy}","received_events_url":"https://api.github.com/users/chrisdembia/received_events","type":"User","site_admin":false},"body":"> with slop (tolerance) > 1e-6 you would expect this to return 3, yes?\r\n\r\nTo answer your question directly, yes. This test is checking the behavior when the slop is < 1e-6, for sanity.","created_at":"2016-02-08T06:00:35Z","updated_at":"2016-02-08T06:00:35Z","html_url":"https://github.com/opensim-org/opensim-core/pull/730#discussion_r52135380","pull_request_url":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730","_links":{"self":{"href":"https://api.github.com/repos/opensim-org/opensim-core/pulls/comments/52135380"},"html":{"href":"https://github.com/opensim-org/opensim-core/pull/730#discussion_r52135380"},"pull_request":{"href":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730"}}},"pull_request":{"url":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730","id":54688570,"html_url":"https://github.com/opensim-org/opensim-core/pull/730","diff_url":"https://github.com/opensim-org/opensim-core/pull/730.diff","patch_url":"https://github.com/opensim-org/opensim-core/pull/730.patch","issue_url":"https://api.github.com/repos/opensim-org/opensim-core/issues/730","number":730,"state":"open","locked":false,"title":"StatesTrajectory","user":{"login":"chrisdembia","id":846001,"avatar_url":"https://avatars.githubusercontent.com/u/846001?v=3","gravatar_id":"","url":"https://api.github.com/users/chrisdembia","html_url":"https://github.com/chrisdembia","followers_url":"https://api.github.com/users/chrisdembia/followers","following_url":"https://api.github.com/users/chrisdembia/following{/other_user}","gists_url":"https://api.github.com/users/chrisdembia/gists{/gist_id}","starred_url":"https://api.github.com/users/chrisdembia/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/chrisdembia/subscriptions","organizations_url":"https://api.github.com/users/chrisdembia/orgs","repos_url":"https://api.github.com/users/chrisdembia/repos","events_url":"https://api.github.com/users/chrisdembia/events{/privacy}","received_events_url":"https://api.github.com/users/chrisdembia/received_events","type":"User","site_admin":false},"body":"I am actively working on a StatesTrajectory class. It's far from done, but I wanted to create a PR in case anyone has early feedback.\r\n\r\nEDIT: This PR is now ready for review. \r\n\r\n## Summary\r\n\r\n1. I've added a class StatesTrajectory (though there is no way to serialize/deserialize it yet).\r\n2. The class has a static method that can populate the trajectory from a states storage file.\r\n3. I've wrapped the class for scripting.\r\n4. In python, one can also iterate through a trajectory:\r\n  ```py\r\n  states = osim.StatesTrajectory.createFromStatesStorage(\r\n                   model, \"subject01_states.sto\")\r\n  for state in states:\r\n      model.calcMassCenterPosition(state)\r\n  ```\r\n\r\n5. I've written C++ and python tests.\r\n6. I also created a sandbox file that compiles and runs without issue, to serve as example code:\r\n  ```cpp\r\n     Model model(\"gait10dof18musc_subject01.osim\");\r\n     ...\r\n     auto& initState = model.initSystem();\r\n     StatesTrajectory states = simulate(model, initState, 0.05);\r\n \r\n     // Use trajectory.\r\n     // ---------------\r\n     for (const auto& state : states) {\r\n         std::cout << state.getTime() << \"s: \"\r\n                   << model.getStateVariableValue(state,\r\n                                                  \"ankle_r/ankle_angle_r/value\")\r\n                   << std::endl;\r\n     }\r\n  ```\r\n\r\n7. The doxygen for this class is available at http://myosin.sourceforge.net/730/classOpenSim_1_1StatesTrajectory.html.\r\n8. I made some small changes to Storage to support what I was doing with the trajectory.\r\n\r\n## Discussion points\r\n1. It may seem that a StatesTrajectory class belongs in Simbody. However, I talked with @sherm1 about this for a while and we decided that a sequential container of states is not too useful for simbody users, and that putting the container in OpenSim lets us make it more specific/powerful for OpenSim users. The idea is that Simbody will be able to de/serialize individual states as elements in an XML file, and StatesTrajectory's de/serialization will leverage that.\r\n2. When appending a new state to the trajectory, I check that the time of the new state is greater/equal to that of the last state. However, it's easy to alter the trajectory so that the times are not nondecreasing (e.g., `states[5].setTime(5)`). So we have 3 options: (1) drop any notion that the times are nondecreasing, (2) leave as-is with the weak enforcement, (3) find a way to truly enforce an invariant that the times are nondecreasing.\r\n3. What about creating a StatesTrajectory from kinematics storages (or kinematics and speed storages)? I don't think we should do this; instead, we should have a formal concept of \"trajectory builders\", like IK, muscle equilibrium, etc., and one could be creating a trajectory from just kinematics info (e.g., IK output).\r\n4. Should cache variables be saved in the serialization? Some cache variables would be silly to recompute (CMC controls) and thus make sense to save). This question is a little bit beyond the scope of this PR.\r\n5. Should `createFromStatesStorage()` have an option to `assemble()` each state?\r\n6. I created a method `consistent()` that checks if all the states have the same number of Q's, U's, etc. The guts of this method really belong in Simbody. Do we all agree? Should the `consistent()` method even exist?\r\n7. File extension for trajectory files? In the past I've like 'osimstates', but I ended up going with Ajay's 'ost'. We should discuss, though, since there might be even more opensim-related file extensions in the future.\r\n\r\nEDIT: TODO after Ajay's first review.\r\n\r\n- [x] \"results with greater ease and more versatility?\"\r\n- [x] Remove `TODO acceleration-level calculations?`\r\n- [x] Un-capitalize `.OSTATES`.\r\n- [x] Update description of cache variables.\r\n- [x] Update `Model::initSystem()` warning (explain why this call is essential).\r\n- [x] Nested exception classes #755.\r\n- [x] \".. (at time = \" ...\"\r\n- [x] \"should be set 'inDegrees=' to no! right?\"\r\n- [x] Storage::getStateIndex()\r\n- [x] StatesTrajectory::createFromStatesStorage() value for unspecified columns (NaN).\r\n- [x] Exception constructors; default argument `msg = \"\"`.\r\n- [ ] \"why is time not strictly increasing?\"\r\n- [ ] Default argument for tolerance; what should the default tolerance be?\r\n- [ ] \"with slop (tolerance) > 1e-6 you would expect this to return 3, yes? maybe 'getIndexAtOrBefore(time)' would better match the use/intent?\"\r\n- [ ] Python test: \"Isn't the use case that actual states are truncated and have a loss of precision (e.g. 4.99999999999) but very close to user intended start/end times that are crisp/precise (like 5)?\"\r\n- [ ] `getIndexByTime(time, timematchoption, tolerance)`","created_at":"2015-12-28T22:26:34Z","updated_at":"2016-02-08T06:00:35Z","closed_at":null,"merged_at":null,"merge_commit_sha":"309e1a3d9ac1342cfb0f57f7de26094a187146ad","assignee":null,"milestone":null,"commits_url":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730/commits","review_comments_url":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730/comments","review_comment_url":"https://api.github.com/repos/opensim-org/opensim-core/pulls/comments{/number}","comments_url":"https://api.github.com/repos/opensim-org/opensim-core/issues/730/comments","statuses_url":"https://api.github.com/repos/opensim-org/opensim-core/statuses/7ffd6179556c79d4491c072f06cd568c7dafc201","head":{"label":"opensim-org:states-trajectory","ref":"states-trajectory","sha":"7ffd6179556c79d4491c072f06cd568c7dafc201","user":{"login":"opensim-org","id":4370782,"avatar_url":"https://avatars.githubusercontent.com/u/4370782?v=3","gravatar_id":"","url":"https://api.github.com/users/opensim-org","html_url":"https://github.com/opensim-org","followers_url":"https://api.github.com/users/opensim-org/followers","following_url":"https://api.github.com/users/opensim-org/following{/other_user}","gists_url":"https://api.github.com/users/opensim-org/gists{/gist_id}","starred_url":"https://api.github.com/users/opensim-org/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/opensim-org/subscriptions","organizations_url":"https://api.github.com/users/opensim-org/orgs","repos_url":"https://api.github.com/users/opensim-org/repos","events_url":"https://api.github.com/users/opensim-org/events{/privacy}","received_events_url":"https://api.github.com/users/opensim-org/received_events","type":"Organization","site_admin":false},"repo":{"id":20775600,"name":"opensim-core","full_name":"opensim-org/opensim-core","owner":{"login":"opensim-org","id":4370782,"avatar_url":"https://avatars.githubusercontent.com/u/4370782?v=3","gravatar_id":"","url":"https://api.github.com/users/opensim-org","html_url":"https://github.com/opensim-org","followers_url":"https://api.github.com/users/opensim-org/followers","following_url":"https://api.github.com/users/opensim-org/following{/other_user}","gists_url":"https://api.github.com/users/opensim-org/gists{/gist_id}","starred_url":"https://api.github.com/users/opensim-org/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/opensim-org/subscriptions","organizations_url":"https://api.github.com/users/opensim-org/orgs","repos_url":"https://api.github.com/users/opensim-org/repos","events_url":"https://api.github.com/users/opensim-org/events{/privacy}","received_events_url":"https://api.github.com/users/opensim-org/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/opensim-org/opensim-core","description":"SimTK OpenSim 4.0 C++ libraries and command-line applications, and Java/Python wrapping.","fork":false,"url":"https://api.github.com/repos/opensim-org/opensim-core","forks_url":"https://api.github.com/repos/opensim-org/opensim-core/forks","keys_url":"https://api.github.com/repos/opensim-org/opensim-core/keys{/key_id}","collaborators_url":"https://api.github.com/repos/opensim-org/opensim-core/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/opensim-org/opensim-core/teams","hooks_url":"https://api.github.com/repos/opensim-org/opensim-core/hooks","issue_events_url":"https://api.github.com/repos/opensim-org/opensim-core/issues/events{/number}","events_url":"https://api.github.com/repos/opensim-org/opensim-core/events","assignees_url":"https://api.github.com/repos/opensim-org/opensim-core/assignees{/user}","branches_url":"https://api.github.com/repos/opensim-org/opensim-core/branches{/branch}","tags_url":"https://api.github.com/repos/opensim-org/opensim-core/tags","blobs_url":"https://api.github.com/repos/opensim-org/opensim-core/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/opensim-org/opensim-core/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/opensim-org/opensim-core/git/refs{/sha}","trees_url":"https://api.github.com/repos/opensim-org/opensim-core/git/trees{/sha}","statuses_url":"https://api.github.com/repos/opensim-org/opensim-core/statuses/{sha}","languages_url":"https://api.github.com/repos/opensim-org/opensim-core/languages","stargazers_url":"https://api.github.com/repos/opensim-org/opensim-core/stargazers","contributors_url":"https://api.github.com/repos/opensim-org/opensim-core/contributors","subscribers_url":"https://api.github.com/repos/opensim-org/opensim-core/subscribers","subscription_url":"https://api.github.com/repos/opensim-org/opensim-core/subscription","commits_url":"https://api.github.com/repos/opensim-org/opensim-core/commits{/sha}","git_commits_url":"https://api.github.com/repos/opensim-org/opensim-core/git/commits{/sha}","comments_url":"https://api.github.com/repos/opensim-org/opensim-core/comments{/number}","issue_comment_url":"https://api.github.com/repos/opensim-org/opensim-core/issues/comments{/number}","contents_url":"https://api.github.com/repos/opensim-org/opensim-core/contents/{+path}","compare_url":"https://api.github.com/repos/opensim-org/opensim-core/compare/{base}...{head}","merges_url":"https://api.github.com/repos/opensim-org/opensim-core/merges","archive_url":"https://api.github.com/repos/opensim-org/opensim-core/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/opensim-org/opensim-core/downloads","issues_url":"https://api.github.com/repos/opensim-org/opensim-core/issues{/number}","pulls_url":"https://api.github.com/repos/opensim-org/opensim-core/pulls{/number}","milestones_url":"https://api.github.com/repos/opensim-org/opensim-core/milestones{/number}","notifications_url":"https://api.github.com/repos/opensim-org/opensim-core/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/opensim-org/opensim-core/labels{/name}","releases_url":"https://api.github.com/repos/opensim-org/opensim-core/releases{/id}","deployments_url":"https://api.github.com/repos/opensim-org/opensim-core/deployments","created_at":"2014-06-12T16:57:56Z","updated_at":"2016-01-29T14:01:56Z","pushed_at":"2016-02-08T03:45:54Z","git_url":"git://github.com/opensim-org/opensim-core.git","ssh_url":"git@github.com:opensim-org/opensim-core.git","clone_url":"https://github.com/opensim-org/opensim-core.git","svn_url":"https://github.com/opensim-org/opensim-core","homepage":"https://opensim.stanford.edu","size":127393,"stargazers_count":26,"watchers_count":26,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":20,"mirror_url":null,"open_issues_count":238,"forks":20,"open_issues":238,"watchers":26,"default_branch":"master"}},"base":{"label":"opensim-org:master","ref":"master","sha":"f4a03a5123a49045666cd0f40ff04b4cfa087a5e","user":{"login":"opensim-org","id":4370782,"avatar_url":"https://avatars.githubusercontent.com/u/4370782?v=3","gravatar_id":"","url":"https://api.github.com/users/opensim-org","html_url":"https://github.com/opensim-org","followers_url":"https://api.github.com/users/opensim-org/followers","following_url":"https://api.github.com/users/opensim-org/following{/other_user}","gists_url":"https://api.github.com/users/opensim-org/gists{/gist_id}","starred_url":"https://api.github.com/users/opensim-org/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/opensim-org/subscriptions","organizations_url":"https://api.github.com/users/opensim-org/orgs","repos_url":"https://api.github.com/users/opensim-org/repos","events_url":"https://api.github.com/users/opensim-org/events{/privacy}","received_events_url":"https://api.github.com/users/opensim-org/received_events","type":"Organization","site_admin":false},"repo":{"id":20775600,"name":"opensim-core","full_name":"opensim-org/opensim-core","owner":{"login":"opensim-org","id":4370782,"avatar_url":"https://avatars.githubusercontent.com/u/4370782?v=3","gravatar_id":"","url":"https://api.github.com/users/opensim-org","html_url":"https://github.com/opensim-org","followers_url":"https://api.github.com/users/opensim-org/followers","following_url":"https://api.github.com/users/opensim-org/following{/other_user}","gists_url":"https://api.github.com/users/opensim-org/gists{/gist_id}","starred_url":"https://api.github.com/users/opensim-org/starred{/owner}{/repo}","subscriptions_url":"https://api.github.com/users/opensim-org/subscriptions","organizations_url":"https://api.github.com/users/opensim-org/orgs","repos_url":"https://api.github.com/users/opensim-org/repos","events_url":"https://api.github.com/users/opensim-org/events{/privacy}","received_events_url":"https://api.github.com/users/opensim-org/received_events","type":"Organization","site_admin":false},"private":false,"html_url":"https://github.com/opensim-org/opensim-core","description":"SimTK OpenSim 4.0 C++ libraries and command-line applications, and Java/Python wrapping.","fork":false,"url":"https://api.github.com/repos/opensim-org/opensim-core","forks_url":"https://api.github.com/repos/opensim-org/opensim-core/forks","keys_url":"https://api.github.com/repos/opensim-org/opensim-core/keys{/key_id}","collaborators_url":"https://api.github.com/repos/opensim-org/opensim-core/collaborators{/collaborator}","teams_url":"https://api.github.com/repos/opensim-org/opensim-core/teams","hooks_url":"https://api.github.com/repos/opensim-org/opensim-core/hooks","issue_events_url":"https://api.github.com/repos/opensim-org/opensim-core/issues/events{/number}","events_url":"https://api.github.com/repos/opensim-org/opensim-core/events","assignees_url":"https://api.github.com/repos/opensim-org/opensim-core/assignees{/user}","branches_url":"https://api.github.com/repos/opensim-org/opensim-core/branches{/branch}","tags_url":"https://api.github.com/repos/opensim-org/opensim-core/tags","blobs_url":"https://api.github.com/repos/opensim-org/opensim-core/git/blobs{/sha}","git_tags_url":"https://api.github.com/repos/opensim-org/opensim-core/git/tags{/sha}","git_refs_url":"https://api.github.com/repos/opensim-org/opensim-core/git/refs{/sha}","trees_url":"https://api.github.com/repos/opensim-org/opensim-core/git/trees{/sha}","statuses_url":"https://api.github.com/repos/opensim-org/opensim-core/statuses/{sha}","languages_url":"https://api.github.com/repos/opensim-org/opensim-core/languages","stargazers_url":"https://api.github.com/repos/opensim-org/opensim-core/stargazers","contributors_url":"https://api.github.com/repos/opensim-org/opensim-core/contributors","subscribers_url":"https://api.github.com/repos/opensim-org/opensim-core/subscribers","subscription_url":"https://api.github.com/repos/opensim-org/opensim-core/subscription","commits_url":"https://api.github.com/repos/opensim-org/opensim-core/commits{/sha}","git_commits_url":"https://api.github.com/repos/opensim-org/opensim-core/git/commits{/sha}","comments_url":"https://api.github.com/repos/opensim-org/opensim-core/comments{/number}","issue_comment_url":"https://api.github.com/repos/opensim-org/opensim-core/issues/comments{/number}","contents_url":"https://api.github.com/repos/opensim-org/opensim-core/contents/{+path}","compare_url":"https://api.github.com/repos/opensim-org/opensim-core/compare/{base}...{head}","merges_url":"https://api.github.com/repos/opensim-org/opensim-core/merges","archive_url":"https://api.github.com/repos/opensim-org/opensim-core/{archive_format}{/ref}","downloads_url":"https://api.github.com/repos/opensim-org/opensim-core/downloads","issues_url":"https://api.github.com/repos/opensim-org/opensim-core/issues{/number}","pulls_url":"https://api.github.com/repos/opensim-org/opensim-core/pulls{/number}","milestones_url":"https://api.github.com/repos/opensim-org/opensim-core/milestones{/number}","notifications_url":"https://api.github.com/repos/opensim-org/opensim-core/notifications{?since,all,participating}","labels_url":"https://api.github.com/repos/opensim-org/opensim-core/labels{/name}","releases_url":"https://api.github.com/repos/opensim-org/opensim-core/releases{/id}","deployments_url":"https://api.github.com/repos/opensim-org/opensim-core/deployments","created_at":"2014-06-12T16:57:56Z","updated_at":"2016-01-29T14:01:56Z","pushed_at":"2016-02-08T03:45:54Z","git_url":"git://github.com/opensim-org/opensim-core.git","ssh_url":"git@github.com:opensim-org/opensim-core.git","clone_url":"https://github.com/opensim-org/opensim-core.git","svn_url":"https://github.com/opensim-org/opensim-core","homepage":"https://opensim.stanford.edu","size":127393,"stargazers_count":26,"watchers_count":26,"language":"C++","has_issues":true,"has_downloads":true,"has_wiki":true,"has_pages":false,"forks_count":20,"mirror_url":null,"open_issues_count":238,"forks":20,"open_issues":238,"watchers":26,"default_branch":"master"}},"_links":{"self":{"href":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730"},"html":{"href":"https://github.com/opensim-org/opensim-core/pull/730"},"issue":{"href":"https://api.github.com/repos/opensim-org/opensim-core/issues/730"},"comments":{"href":"https://api.github.com/repos/opensim-org/opensim-core/issues/730/comments"},"review_comments":{"href":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730/comments"},"review_comment":{"href":"https://api.github.com/repos/opensim-org/opensim-core/pulls/comments{/number}"},"commits":{"href":"https://api.github.com/repos/opensim-org/opensim-core/pulls/730/commits"},"statuses":{"href":"https://api.github.com/repos/opensim-org/opensim-core/statuses/7ffd6179556c79d4491c072f06cd568c7dafc201"}}}}