From 4514fbaf12896431540354c7dffc08bbfe935643 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 18 Sep 2023 15:00:11 -0700 Subject: [PATCH 01/34] Draft utilities for adding FunctionBasedPaths --- OpenSim/Actuators/ModelFactory.cpp | 1 - OpenSim/Actuators/ModelFactory.h | 76 +++++++++++- OpenSim/Actuators/ModelOperators.h | 113 ++++++++++++++++++ .../Actuators/RegisterTypes_osimActuators.cpp | 2 + 4 files changed, 190 insertions(+), 2 deletions(-) diff --git a/OpenSim/Actuators/ModelFactory.cpp b/OpenSim/Actuators/ModelFactory.cpp index 4cacb6980b..c61d6f6c5b 100644 --- a/OpenSim/Actuators/ModelFactory.cpp +++ b/OpenSim/Actuators/ModelFactory.cpp @@ -308,4 +308,3 @@ void ModelFactory::createReserveActuators(Model& model, double optimalForce, log_info(" {}", name); } } - diff --git a/OpenSim/Actuators/ModelFactory.h b/OpenSim/Actuators/ModelFactory.h index 1d0b907791..9cc929df9e 100644 --- a/OpenSim/Actuators/ModelFactory.h +++ b/OpenSim/Actuators/ModelFactory.h @@ -20,6 +20,7 @@ #include "osimActuatorsDLL.h" #include +#include namespace OpenSim { @@ -89,7 +90,80 @@ class OSIMACTUATORS_API ModelFactory { static void createReserveActuators(Model& model, double optimalForce, double bound = SimTK::NaN, bool skipCoordinatesWithExistingActuators = true); - + + /// Replace the paths of the forces in the model with the + /// FunctionBasedPath%s specified in the file 'pathsFileName'. The file must + /// be a Set of FunctionBasedPath%s where the name of each path matches the + /// name of a corresponding Force in the model. The path name is appended + /// with "_path" to avoid name ambiguity in the final model. + /// @note This checks both the model's ForceSet and the model's components + /// list for forces matches the names of the FunctionBasedPath%s in the + /// file. + template + static void replacePathsWithFunctionBasedPaths(Model& model, + const std::string& pathsFileName) { + Set pathSet(pathsFileName); + for (int i = 0; i < pathSet.getSize(); ++i) { + auto path = pathSet.get(i); + const auto& forceName = path.getName(); + const auto componentPath = fmt::format("/{}", forceName); + const auto forceSetPath = fmt::format("/forceset/{}", forceName); + OPENSIM_THROW_IF(!model.hasComponent(forceSetPath) && + !model.hasComponent(componentPath), Exception, + "Model does not contain a force with name {}.", forceName); + OPENSIM_THROW_IF(model.hasComponent(forceSetPath) && + model.hasComponent(componentPath), Exception, + "Model does not contain a force with name {} at two " + "paths: {} and {}", forceName, forceSetPath, componentPath); + + // Get the force component. + const auto forcePath = model.hasComponent(forceSetPath) ? + forceSetPath : componentPath; + auto& force = model.updComponent(forcePath); + + // Check that the force has a path property. + OPENSIM_THROW_IF( + !force.template hasProperty(), Exception, + "Force {} does not have a path property.", forceName); + + // Overwrite the path with the function-based path. + path.setName(fmt::format("{}_path", forceName))) + force.set_path(path); + } + model.finalizeConnections(); + } + + /// @copydoc replacePathsWithFunctionBasedPaths() + static void replacePathSpringPathsWithFunctionBasedPaths(Model& model, + const std::string& pathsFileName) { + replacePathsWithFunctionBasedPaths(model, pathsFileName); + } + + /// @copydoc replacePathsWithFunctionBasedPaths() + /// @note This will replace the paths of all Muscle%s in the model. + static void replacePathActuatorPathsWithFunctionBasedPaths(Model& model, + const std::string& pathsFileName) { + replacePathsWithFunctionBasedPaths(model, pathsFileName); + } + + /// @copydoc replacePathsWithFunctionBasedPaths() + static void replaceMusclePathsWithFunctionBasedPaths(Model& model, + const std::string& pathsFileName) { + replacePathsWithFunctionBasedPaths(model, pathsFileName); + } + + /// @copydoc replacePathsWithFunctionBasedPaths() + static void replaceLigamentPathsWithFunctionBasedPaths(Model& model, + const std::string& pathsFileName) { + replacePathsWithFunctionBasedPaths(model, pathsFileName); + } + + /// @copydoc replacePathsWithFunctionBasedPaths() + static void replaceBlankevoort1991LigamentPathsWithFunctionBasedPaths( + Model& model, const std::string& pathsFileName) { + replacePathsWithFunctionBasedPaths( + model, pathsFileName); + } }; } // namespace OpenSim diff --git a/OpenSim/Actuators/ModelOperators.h b/OpenSim/Actuators/ModelOperators.h index add082e825..3af2cb0f59 100644 --- a/OpenSim/Actuators/ModelOperators.h +++ b/OpenSim/Actuators/ModelOperators.h @@ -188,6 +188,119 @@ class OSIMACTUATORS_API ModOpReplaceMusclesWithPathActuators } }; +class OSIMACTUATORS_API ModOpReplacePathsWithFunctionBasedPathsBase + : public ModelOperator { + OpenSim_DECLARE_ABSTRACT_OBJECT( + ModOpReplacePathsWithFunctionBasedPathsBase, ModelOperator); + OpenSim_DECLARE_PROPERTY(paths_file, std::string, + "Path to a file containing FunctionBasedPath definitions."); + +public: + ModOpReplacePathsWithFunctionBasedPathsBase() { + constructProperty_paths_file(""); + } + ModOpReplacePathsWithFunctionBasedPathsBase(std::string pathsFile) : + ModOpReplacePathsWithFunctionBasedPathsBase() { + set_paths_file(std::move(pathsFile)); + } + const std::string& getPathsFile() const { return get_paths_file(); } + void operate(Model& model, const std::string&) const override = 0; +}; + +/// Invoke ModelFactory::replacePathSpringPathsWithFunctionBasedPaths() on the +/// model. +class OSIMACTUATORS_API ModOpReplacePathSpringPathsWithFunctionBasedPaths + : public ModOpReplacePathsWithFunctionBasedPathsBase { + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpReplacePathSpringPathsWithFunctionBasedPaths, + ModOpReplacePathsWithFunctionBasedPathsBase); + +public: + void operate(Model& model, const std::string&) const override { + // Without finalizeFromProperties(), an exception is raised + // about the model not having any subcomponents. + model.finalizeFromProperties(); + model.finalizeConnections(); + ModelFactory::replacePathSpringPathsWithFunctionBasedPaths( + model, getPathsFile()); + } +}; + +/// Invoke ModelFactory::replacePathActuatorPathsWithFunctionBasedPaths() on the +/// model. +/// @note This will replace the paths of all Muscle%s in the model. +class OSIMACTUATORS_API ModOpReplacePathActuatorPathsWithFunctionBasedPaths + : public ModOpReplacePathsWithFunctionBasedPathsBase { + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpReplacePathActuatorPathsWithFunctionBasedPaths, + ModOpReplacePathsWithFunctionBasedPathsBase); +public: + void operate(Model& model, const std::string&) const override { + // Without finalizeFromProperties(), an exception is raised + // about the model not having any subcomponents. + model.finalizeFromProperties(); + model.finalizeConnections(); + ModelFactory::replacePathActuatorPathsWithFunctionBasedPaths( + model, getPathsFile()); + } +}; + +/// Invoke ModelFactory::replaceMusclePathsWithFunctionBasedPaths() on the +/// model. +class OSIMACTUATORS_API ModOpReplaceMusclePathsWithFunctionBasedPaths + : public ModOpReplacePathsWithFunctionBasedPathsBase { + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpReplaceMusclePathsWithFunctionBasedPaths, + ModOpReplacePathsWithFunctionBasedPathsBase); +public: + void operate(Model& model, const std::string&) const override { + // Without finalizeFromProperties(), an exception is raised + // about the model not having any subcomponents. + model.finalizeFromProperties(); + model.finalizeConnections(); + ModelFactory::replaceMusclePathsWithFunctionBasedPaths( + model, getPathsFile()); + } +}; + +/// Invoke ModelFactory::replaceLigamentPathsWithFunctionBasedPaths() on the +/// model. +class OSIMACTUATORS_API ModOpReplaceLigamentPathsWithFunctionBasedPaths + : public ModOpReplacePathsWithFunctionBasedPathsBase { + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpReplaceLigamentPathsWithFunctionBasedPaths, + ModOpReplacePathsWithFunctionBasedPathsBase); +public: + void operate(Model& model, const std::string&) const override { + // Without finalizeFromProperties(), an exception is raised + // about the model not having any subcomponents. + model.finalizeFromProperties(); + model.finalizeConnections(); + ModelFactory::replaceLigamentPathsWithFunctionBasedPaths( + model, getPathsFile()); + } +}; + +/// Invoke +/// ModelFactory::replaceBlankevoort1991LigamentPathsWithFunctionBasedPaths() on +/// the model. +class OSIMACTUATORS_API + ModOpReplaceBlankevoort1991LigamentPathsWithFunctionBasedPaths + : public ModOpReplacePathsWithFunctionBasedPathsBase { + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpReplaceBlankevoort1991LigamentPathsWithFunctionBasedPaths, + ModOpReplacePathsWithFunctionBasedPathsBase); +public: + void operate(Model& model, const std::string&) const override { + // Without finalizeFromProperties(), an exception is raised + // about the model not having any subcomponents. + model.finalizeFromProperties(); + model.finalizeConnections(); + ModelFactory::replaceBlankevoort1991LigamentPathsWithFunctionBasedPaths( + model, getPathsFile()); + } +}; + } // namespace OpenSim #endif // OPENSIM_MODELOPERATORS_H diff --git a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp index 52823930f8..50aa3f5bcd 100644 --- a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp +++ b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp @@ -88,6 +88,8 @@ OSIMACTUATORS_API void RegisterTypes_osimActuators() Object::registerType(ModOpAddReserves()); Object::registerType(ModOpAddExternalLoads()); Object::registerType(ModOpReplaceJointsWithWelds()); + Object::registerType(ModOpReplaceMusclesWithPathActuators()); + Object::registerType(ModOpReplacePathSpringPathsWithFunctionBasedPaths()); //Object::RegisterType( ConstantMuscleActivation() ); //Object::RegisterType( ZerothOrderMuscleActivationDynamics() ); From 4b48e743bb9cf4118583f2e2b1876129b86cefa0 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 20 Sep 2023 16:42:37 -0700 Subject: [PATCH 02/34] Fleshing out path utilities --- OpenSim/Actuators/ModelFactory.h | 3 +- .../RegisterTypes_osimSimulation.cpp | 1 + OpenSim/Simulation/SimulationUtilities.cpp | 183 +++++++++++++++++- OpenSim/Simulation/SimulationUtilities.h | 20 ++ OpenSim/Simulation/TableProcessor.h | 18 ++ 5 files changed, 223 insertions(+), 2 deletions(-) diff --git a/OpenSim/Actuators/ModelFactory.h b/OpenSim/Actuators/ModelFactory.h index 9cc929df9e..3d1b317288 100644 --- a/OpenSim/Actuators/ModelFactory.h +++ b/OpenSim/Actuators/ModelFactory.h @@ -19,8 +19,9 @@ * -------------------------------------------------------------------------- */ #include "osimActuatorsDLL.h" -#include #include +#include +#include namespace OpenSim { diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 4ac2a3f7d0..1af93d0496 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -290,6 +290,7 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( TabOpLowPassFilter() ); Object::registerType( TabOpUseAbsoluteStateNames() ); + Object::registerType( TabOpAppendCoupledCoordinateValues() ); Object::registerType( PositionMotion() ); // OLD Versions diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 9b2d4f94d6..bff8661d10 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -27,8 +27,13 @@ #include "Model/Model.h" #include +#include #include +#include +#include + +#include using namespace OpenSim; @@ -437,4 +442,180 @@ TimeSeriesTableVec3 OpenSim::createSyntheticIMUAccelerationSignals( accelTableIMU.setColumnLabels(framePaths); return accelTableIMU; -} \ No newline at end of file +} + +void OpenSim::appendCoupledCoordinateValues( + OpenSim::TimeSeriesTable& table, const OpenSim::Model& model) { + + const CoordinateSet& coordinateSet = model.getCoordinateSet(); + const auto& couplerConstraints = + model.getComponentList(); + for (const auto& couplerConstraint : couplerConstraints) { + + // Get the dependent coordinate and check if the table already contains + // values for it. If so, skip this constraint. + const Coordinate& coordinate = coordinateSet.get( + couplerConstraint.getDependentCoordinateName()); + const std::string& coupledCoordinatePath = + fmt::format("{}/value", coordinate.getAbsolutePathString()); + if (table.hasColumn(coupledCoordinatePath)) { + continue; + } + + // Get the paths to the independent coordinate values. + const Array& independentCoordinateNames = + couplerConstraint.getIndependentCoordinateNames(); + std::vector independentCoordinatePaths; + for (int i = 0; i < independentCoordinateNames.getSize(); ++i) { + const Coordinate& independentCoordinate = coordinateSet.get( + independentCoordinateNames[i]); + independentCoordinatePaths.push_back( + fmt::format("{}/value", + independentCoordinate.getAbsolutePathString())); + OPENSIM_THROW_IF( + !table.hasColumn(independentCoordinatePaths.back()), + Exception, + "Expected the coordinates table to contain a column with " + "label '{}', but it does not.", + independentCoordinatePaths.back()) + } + + // Compute the dependent coordinate values from the function in the + // CoordinateCouplerConstraint. + SimTK::Vector independentValues( + (int)independentCoordinatePaths.size(), 0.0); + SimTK::Vector newColumn((int)table.getNumRows()); + const Function& function = couplerConstraint.getFunction(); + for (int irow = 0; irow < table.getNumRows(); ++irow) { + int ival = 0; + for (const auto& independentCoordinatePath : + independentCoordinatePaths) { + independentValues[ival++] = + table.getDependentColumn(independentCoordinatePath)[irow]; + } + newColumn[irow] = function.calcValue(independentValues); + } + + // Append the new column to the table. + table.appendColumn(coupledCoordinatePath, newColumn); + } +} + + + +void OpenSim::computePathLengthsAndMomentArms( + const std::string& modelFile, + const std::string& coordinateValuesFile, + const std::string& modelName, + const std::string& pathMotionFile4Polynomials, + const std::vector& joints, + const std::vector& muscles, + const std::string& type_bounds_polynomials, + const std::string& side, + int threads = std::thread::hardware_concurrency() - 2) { + + // Check inputs. + OPENSIM_THROW_IF( + threads < 1 || threads > std::thread::hardware_concurrency(), + Exception, "Number of threads must be between 1 and {}.", + std::thread::hardware_concurrency()); + + // Load model. + Model model(modelFile); + model.initSystem(); + + + // Load coordinate values. + TableProcessor tableProcessor = TableProcessor(coordinateValuesFile) | + TabOpUseAbsoluteStateNames() | + TabOpAppendCoupledCoordinateValues(); + TimeSeriesTable coordinateValues = + tableProcessor.processAndConvertToRadians(model); + auto statesTrajectory = StatesTrajectory::createFromStatesTable( + model, coordinateValues); + + // Determine the maximum number of path and moment arm evaluations. + const auto& paths = model.getComponentList(); + int numPaths = std::distance(paths.begin(), paths.end()); + int numCoordinates = coordinateValues.getNumColumns(); + int numColumns = numCoordinates + (numPaths * numCoordinates); + + // Define helper function for path length and moment arm computations. + auto calcPathLengthsAndMomentArmsSubset = [numColumns](Model model, + StatesTrajectory::IteratorRange subsetStates) -> SimTK::Matrix { + Logger::setLevel(Logger::Level::Error); + model.initSystem(); + const CoordinateSet& coordinateSet = model.getCoordinateSet(); + + // Create a matrix to store the results (adjust the dimensions accordingly) + SimTK::Matrix results( + std::distance(subsetStates.begin(), subsetStates.end()), + numColumns); + + int row = 0; + for (const auto& state : subsetStates) { + model.realizePosition(state); + + // Compute and store path lengths and moment arms in the 'results' matrix + // You need to implement the logic to calculate and fill the matrix here. + // You may use 'row' as the row index to store results for each state. + + // Example: + // results(row, 0) = ...; // Store path length for this state + // results(row, 1) = ...; // Store moment arms for this state + + row++; + } + + return results; + }; + + + // Divide the path length and moment arm computations across multiple + // threads. + int stride = static_cast( + std::floor(coordinateValues.getNumRows() / threads)); + std::vector> futures; + int offset = 0; + for (int ithread = 0; ithread < threads; ++ithread) { + StatesTrajectory::const_iterator begin_iter = + statesTrajectory.begin() + offset; + StatesTrajectory::const_iterator end_iter = (ithread == threads-1) ? + statesTrajectory.end() : + statesTrajectory.begin() + offset + stride; + futures.push_back(std::async(std::launch::async, + calcPathLengthsAndMomentArmsSubset, + model, + makeIteratorRange(begin_iter, end_iter))); + offset += stride; + } + + // Wait for threads to finish and collect results + std::vector outputs(threads); + for (int i = 0; i < threads; ++i) { + outputs[i] = futures[i].get(); + } + + // Assemble results into one TimeSeriesTable + + // Gather data + + // Implement logic to combine data from multiple threads + + // Put data in a suitable data structure + + // Save data to file + + // Fit polynomial coefficients + + // You can implement this part based on your needs. + + std::cout << "Fit polynomials." << std::endl; + + // Save polynomialData to file + + + std::cout << "Done fitting polynomials." << std::endl; +} + + diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index e064cef467..99c0d079ee 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -378,6 +378,26 @@ OSIMSIMULATION_API TimeSeriesTableVec3 createSyntheticIMUAccelerationSignals( const TimeSeriesTable& statesTable, const TimeSeriesTable& controlsTable, const std::vector& framePaths); +/// Compute the values of coordinates defined by `CoordinateCouplerConstraint`s +/// in the model and append them to the provided `TimeSeriesTable`. The table +/// should contain columns with values for all the independent coordinates that +/// define the coupled coordinate. The column labels must be the full path to +/// the coordinate values (e.g., `/jointset/ground_pelvis/pelvis_tx/value`). +/// @ingroup simulationutil +OSIMSIMULATION_API void appendCoupledCoordinateValues( + TimeSeriesTable& table, const Model& model); + +OSIMSIMULATION_API void computePathLengthsAndMomentArms( + const std::string& modelFile, + const std::string& coordinateValuesFile, + const std::string& modelName, + const std::string& pathMotionFile4Polynomials, + const std::vector& joints, + const std::vector& muscles, + const std::string& type_bounds_polynomials, + const std::string& side, + int threads); + } // end of namespace OpenSim #endif // OPENSIM_SIMULATION_UTILITIES_H_ diff --git a/OpenSim/Simulation/TableProcessor.h b/OpenSim/Simulation/TableProcessor.h index f3708fc847..1191bb811b 100644 --- a/OpenSim/Simulation/TableProcessor.h +++ b/OpenSim/Simulation/TableProcessor.h @@ -24,6 +24,7 @@ #include #include #include +#include namespace OpenSim { @@ -227,6 +228,23 @@ class OSIMSIMULATION_API TabOpUseAbsoluteStateNames : public TableOperator { } }; +/// Invoke SimulationUtilities::appendCoupledCoordinateValues() on the table. +class OSIMSIMULATION_API TabOpAppendCoupledCoordinateValues + : public TableOperator { + OpenSim_DECLARE_CONCRETE_OBJECT(TabOpAppendCoupledCoordinateValues, + TableOperator); + +public: + TabOpAppendCoupledCoordinateValues() {} + + void operate(TimeSeriesTable& table, const Model* model) const override { + + OPENSIM_THROW_IF(!model, Exception, + "Expected a model, but no model was provided."); + appendCoupledCoordinateValues(table, *model); + } +}; + } // namespace OpenSim #endif // OPENSIM_TABLEPROCESSOR_H From 8a714abba3415f1661645ba3d52bd209eb6fdf36 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 21 Sep 2023 15:01:02 -0700 Subject: [PATCH 03/34] Simplify the new ModelOperator // start fleshing out path length computation code --- OpenSim/Actuators/ModelFactory.h | 68 ++--------- OpenSim/Actuators/ModelOperators.h | 107 ++---------------- .../Actuators/RegisterTypes_osimActuators.cpp | 2 +- OpenSim/Common/Object.h | 9 ++ .../RegisterTypes_osimSimulation.cpp | 2 + OpenSim/Simulation/SimulationUtilities.cpp | 25 +--- OpenSim/Simulation/SimulationUtilities.h | 6 - 7 files changed, 40 insertions(+), 179 deletions(-) diff --git a/OpenSim/Actuators/ModelFactory.h b/OpenSim/Actuators/ModelFactory.h index 3d1b317288..1c2b6b57b2 100644 --- a/OpenSim/Actuators/ModelFactory.h +++ b/OpenSim/Actuators/ModelFactory.h @@ -95,76 +95,32 @@ class OSIMACTUATORS_API ModelFactory { /// Replace the paths of the forces in the model with the /// FunctionBasedPath%s specified in the file 'pathsFileName'. The file must /// be a Set of FunctionBasedPath%s where the name of each path matches the - /// name of a corresponding Force in the model. The path name is appended + /// path of a corresponding Force in the model. The path name is appended /// with "_path" to avoid name ambiguity in the final model. - /// @note This checks both the model's ForceSet and the model's components - /// list for forces matches the names of the FunctionBasedPath%s in the - /// file. - template static void replacePathsWithFunctionBasedPaths(Model& model, const std::string& pathsFileName) { Set pathSet(pathsFileName); for (int i = 0; i < pathSet.getSize(); ++i) { auto path = pathSet.get(i); - const auto& forceName = path.getName(); - const auto componentPath = fmt::format("/{}", forceName); - const auto forceSetPath = fmt::format("/forceset/{}", forceName); - OPENSIM_THROW_IF(!model.hasComponent(forceSetPath) && - !model.hasComponent(componentPath), Exception, - "Model does not contain a force with name {}.", forceName); - OPENSIM_THROW_IF(model.hasComponent(forceSetPath) && - model.hasComponent(componentPath), Exception, - "Model does not contain a force with name {} at two " - "paths: {} and {}", forceName, forceSetPath, componentPath); - // Get the force component. - const auto forcePath = model.hasComponent(forceSetPath) ? - forceSetPath : componentPath; - auto& force = model.updComponent(forcePath); + // Get the force component associated with this path. + OPENSIM_THROW_IF(!model.hasComponent(path.getName()), + Exception, "Model does not contain a Force at path {}.", + path.getName()); + auto& force = model.updComponent(path.getName()); // Check that the force has a path property. OPENSIM_THROW_IF( - !force.template hasProperty(), Exception, - "Force {} does not have a path property.", forceName); + !force.hasProperty("path"), Exception, + "Force {} does not have a path property.", path.getName()); - // Overwrite the path with the function-based path. - path.setName(fmt::format("{}_path", forceName))) - force.set_path(path); + // Update the path. + path.setName(fmt::format("{}_path", force.getName())); + force.updProperty("path").setValue(path); } + model.finalizeFromProperties(); model.finalizeConnections(); } - - /// @copydoc replacePathsWithFunctionBasedPaths() - static void replacePathSpringPathsWithFunctionBasedPaths(Model& model, - const std::string& pathsFileName) { - replacePathsWithFunctionBasedPaths(model, pathsFileName); - } - - /// @copydoc replacePathsWithFunctionBasedPaths() - /// @note This will replace the paths of all Muscle%s in the model. - static void replacePathActuatorPathsWithFunctionBasedPaths(Model& model, - const std::string& pathsFileName) { - replacePathsWithFunctionBasedPaths(model, pathsFileName); - } - - /// @copydoc replacePathsWithFunctionBasedPaths() - static void replaceMusclePathsWithFunctionBasedPaths(Model& model, - const std::string& pathsFileName) { - replacePathsWithFunctionBasedPaths(model, pathsFileName); - } - - /// @copydoc replacePathsWithFunctionBasedPaths() - static void replaceLigamentPathsWithFunctionBasedPaths(Model& model, - const std::string& pathsFileName) { - replacePathsWithFunctionBasedPaths(model, pathsFileName); - } - - /// @copydoc replacePathsWithFunctionBasedPaths() - static void replaceBlankevoort1991LigamentPathsWithFunctionBasedPaths( - Model& model, const std::string& pathsFileName) { - replacePathsWithFunctionBasedPaths( - model, pathsFileName); - } }; } // namespace OpenSim diff --git a/OpenSim/Actuators/ModelOperators.h b/OpenSim/Actuators/ModelOperators.h index 3af2cb0f59..a5b31d03a9 100644 --- a/OpenSim/Actuators/ModelOperators.h +++ b/OpenSim/Actuators/ModelOperators.h @@ -188,116 +188,29 @@ class OSIMACTUATORS_API ModOpReplaceMusclesWithPathActuators } }; -class OSIMACTUATORS_API ModOpReplacePathsWithFunctionBasedPathsBase +/// Invoke ModelFactory::replacePathsWithFunctionBasedPaths() on the model. +class OSIMACTUATORS_API ModOpReplacePathsWithFunctionBasedPaths : public ModelOperator { - OpenSim_DECLARE_ABSTRACT_OBJECT( - ModOpReplacePathsWithFunctionBasedPathsBase, ModelOperator); + OpenSim_DECLARE_CONCRETE_OBJECT( + ModOpReplacePathsWithFunctionBasedPaths, ModelOperator); OpenSim_DECLARE_PROPERTY(paths_file, std::string, "Path to a file containing FunctionBasedPath definitions."); - + public: - ModOpReplacePathsWithFunctionBasedPathsBase() { + ModOpReplacePathsWithFunctionBasedPaths() { constructProperty_paths_file(""); } - ModOpReplacePathsWithFunctionBasedPathsBase(std::string pathsFile) : - ModOpReplacePathsWithFunctionBasedPathsBase() { + ModOpReplacePathsWithFunctionBasedPaths(std::string pathsFile) : + ModOpReplacePathsWithFunctionBasedPaths() { set_paths_file(std::move(pathsFile)); } - const std::string& getPathsFile() const { return get_paths_file(); } - void operate(Model& model, const std::string&) const override = 0; -}; - -/// Invoke ModelFactory::replacePathSpringPathsWithFunctionBasedPaths() on the -/// model. -class OSIMACTUATORS_API ModOpReplacePathSpringPathsWithFunctionBasedPaths - : public ModOpReplacePathsWithFunctionBasedPathsBase { - OpenSim_DECLARE_CONCRETE_OBJECT( - ModOpReplacePathSpringPathsWithFunctionBasedPaths, - ModOpReplacePathsWithFunctionBasedPathsBase); - -public: - void operate(Model& model, const std::string&) const override { - // Without finalizeFromProperties(), an exception is raised - // about the model not having any subcomponents. - model.finalizeFromProperties(); - model.finalizeConnections(); - ModelFactory::replacePathSpringPathsWithFunctionBasedPaths( - model, getPathsFile()); - } -}; - -/// Invoke ModelFactory::replacePathActuatorPathsWithFunctionBasedPaths() on the -/// model. -/// @note This will replace the paths of all Muscle%s in the model. -class OSIMACTUATORS_API ModOpReplacePathActuatorPathsWithFunctionBasedPaths - : public ModOpReplacePathsWithFunctionBasedPathsBase { - OpenSim_DECLARE_CONCRETE_OBJECT( - ModOpReplacePathActuatorPathsWithFunctionBasedPaths, - ModOpReplacePathsWithFunctionBasedPathsBase); -public: - void operate(Model& model, const std::string&) const override { - // Without finalizeFromProperties(), an exception is raised - // about the model not having any subcomponents. - model.finalizeFromProperties(); - model.finalizeConnections(); - ModelFactory::replacePathActuatorPathsWithFunctionBasedPaths( - model, getPathsFile()); - } -}; - -/// Invoke ModelFactory::replaceMusclePathsWithFunctionBasedPaths() on the -/// model. -class OSIMACTUATORS_API ModOpReplaceMusclePathsWithFunctionBasedPaths - : public ModOpReplacePathsWithFunctionBasedPathsBase { - OpenSim_DECLARE_CONCRETE_OBJECT( - ModOpReplaceMusclePathsWithFunctionBasedPaths, - ModOpReplacePathsWithFunctionBasedPathsBase); -public: - void operate(Model& model, const std::string&) const override { - // Without finalizeFromProperties(), an exception is raised - // about the model not having any subcomponents. - model.finalizeFromProperties(); - model.finalizeConnections(); - ModelFactory::replaceMusclePathsWithFunctionBasedPaths( - model, getPathsFile()); - } -}; - -/// Invoke ModelFactory::replaceLigamentPathsWithFunctionBasedPaths() on the -/// model. -class OSIMACTUATORS_API ModOpReplaceLigamentPathsWithFunctionBasedPaths - : public ModOpReplacePathsWithFunctionBasedPathsBase { - OpenSim_DECLARE_CONCRETE_OBJECT( - ModOpReplaceLigamentPathsWithFunctionBasedPaths, - ModOpReplacePathsWithFunctionBasedPathsBase); -public: - void operate(Model& model, const std::string&) const override { - // Without finalizeFromProperties(), an exception is raised - // about the model not having any subcomponents. - model.finalizeFromProperties(); - model.finalizeConnections(); - ModelFactory::replaceLigamentPathsWithFunctionBasedPaths( - model, getPathsFile()); - } -}; - -/// Invoke -/// ModelFactory::replaceBlankevoort1991LigamentPathsWithFunctionBasedPaths() on -/// the model. -class OSIMACTUATORS_API - ModOpReplaceBlankevoort1991LigamentPathsWithFunctionBasedPaths - : public ModOpReplacePathsWithFunctionBasedPathsBase { - OpenSim_DECLARE_CONCRETE_OBJECT( - ModOpReplaceBlankevoort1991LigamentPathsWithFunctionBasedPaths, - ModOpReplacePathsWithFunctionBasedPathsBase); -public: void operate(Model& model, const std::string&) const override { // Without finalizeFromProperties(), an exception is raised // about the model not having any subcomponents. model.finalizeFromProperties(); model.finalizeConnections(); - ModelFactory::replaceBlankevoort1991LigamentPathsWithFunctionBasedPaths( - model, getPathsFile()); + ModelFactory::replacePathsWithFunctionBasedPaths(model, + get_paths_file()); } }; diff --git a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp index 50aa3f5bcd..dff13c0638 100644 --- a/OpenSim/Actuators/RegisterTypes_osimActuators.cpp +++ b/OpenSim/Actuators/RegisterTypes_osimActuators.cpp @@ -89,7 +89,7 @@ OSIMACTUATORS_API void RegisterTypes_osimActuators() Object::registerType(ModOpAddExternalLoads()); Object::registerType(ModOpReplaceJointsWithWelds()); Object::registerType(ModOpReplaceMusclesWithPathActuators()); - Object::registerType(ModOpReplacePathSpringPathsWithFunctionBasedPaths()); + Object::registerType(ModOpReplacePathsWithFunctionBasedPaths()); //Object::RegisterType( ConstantMuscleActivation() ); //Object::RegisterType( ZerothOrderMuscleActivationDynamics() ); diff --git a/OpenSim/Common/Object.h b/OpenSim/Common/Object.h index 8618fa7279..4eee547100 100644 --- a/OpenSim/Common/Object.h +++ b/OpenSim/Common/Object.h @@ -347,6 +347,9 @@ class OSIMCOMMON_API Object used by the Property declaration macros for fast access to properties. **/ template Property& updProperty(const PropertyIndex& index); + /** @copydoc updProperty(const PropertyIndex&) **/ + template Property& + updProperty(const std::string& name); /** Returns \c true if no property's value has changed since the last time setObjectIsUpToDateWithProperties() was called. **/ @@ -943,6 +946,12 @@ updProperty(const PropertyIndex& index) { return _propertyTable.updProperty(index); } +template Property& Object:: +updProperty(const std::string& name) { + _objectIsUpToDate = false; // property may be changed + return _propertyTable.updProperty(name); +} + template PropertyIndex Object:: addProperty(const std::string& name, const std::string& comment, diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 1af93d0496..983e5eb53e 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -60,6 +60,7 @@ #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" #include "Model/GeometryPath.h" +#include "Model/FunctionBasedPath.h" #include "Model/PrescribedForce.h" #include "Model/ExternalForce.h" #include "Model/PointToPointSpring.h" @@ -192,6 +193,7 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( FrameGeometry()); Object::registerType( Arrow()); Object::registerType( GeometryPath()); + Object::registerType( FunctionBasedPath()); Object::registerType( ControlSet() ); Object::registerType( ControlConstant() ); diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index bff8661d10..ac80abfd55 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -506,13 +506,7 @@ void OpenSim::appendCoupledCoordinateValues( void OpenSim::computePathLengthsAndMomentArms( const std::string& modelFile, const std::string& coordinateValuesFile, - const std::string& modelName, - const std::string& pathMotionFile4Polynomials, - const std::vector& joints, - const std::vector& muscles, - const std::string& type_bounds_polynomials, - const std::string& side, - int threads = std::thread::hardware_concurrency() - 2) { + int threads = (int)std::thread::hardware_concurrency() - 2) { // Check inputs. OPENSIM_THROW_IF( @@ -524,7 +518,6 @@ void OpenSim::computePathLengthsAndMomentArms( Model model(modelFile); model.initSystem(); - // Load coordinate values. TableProcessor tableProcessor = TableProcessor(coordinateValuesFile) | TabOpUseAbsoluteStateNames() | @@ -536,8 +529,8 @@ void OpenSim::computePathLengthsAndMomentArms( // Determine the maximum number of path and moment arm evaluations. const auto& paths = model.getComponentList(); - int numPaths = std::distance(paths.begin(), paths.end()); - int numCoordinates = coordinateValues.getNumColumns(); + int numPaths = (int)std::distance(paths.begin(), paths.end()); + int numCoordinates = (int)coordinateValues.getNumColumns(); int numColumns = numCoordinates + (numPaths * numCoordinates); // Define helper function for path length and moment arm computations. @@ -547,22 +540,16 @@ void OpenSim::computePathLengthsAndMomentArms( model.initSystem(); const CoordinateSet& coordinateSet = model.getCoordinateSet(); - // Create a matrix to store the results (adjust the dimensions accordingly) + // Create a matrix to store the results SimTK::Matrix results( - std::distance(subsetStates.begin(), subsetStates.end()), + (int)std::distance(subsetStates.begin(), subsetStates.end()), numColumns); int row = 0; for (const auto& state : subsetStates) { model.realizePosition(state); - // Compute and store path lengths and moment arms in the 'results' matrix - // You need to implement the logic to calculate and fill the matrix here. - // You may use 'row' as the row index to store results for each state. - - // Example: - // results(row, 0) = ...; // Store path length for this state - // results(row, 1) = ...; // Store moment arms for this state + row++; } diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 99c0d079ee..96bc662ad6 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -390,12 +390,6 @@ OSIMSIMULATION_API void appendCoupledCoordinateValues( OSIMSIMULATION_API void computePathLengthsAndMomentArms( const std::string& modelFile, const std::string& coordinateValuesFile, - const std::string& modelName, - const std::string& pathMotionFile4Polynomials, - const std::vector& joints, - const std::vector& muscles, - const std::string& type_bounds_polynomials, - const std::string& side, int threads); } // end of namespace OpenSim From 307a32109c8785b186e9a7247dde37dc94cd08c3 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 22 Sep 2023 11:14:17 -0700 Subject: [PATCH 04/34] finalize path length and moment arm computation utility --- OpenSim/Auxiliary/auxiliaryTestFunctions.h | 35 ++++-- OpenSim/Common/Object.h | 8 ++ OpenSim/Simulation/SimulationUtilities.cpp | 126 +++++++++++++++------ OpenSim/Simulation/SimulationUtilities.h | 22 +++- 4 files changed, 142 insertions(+), 49 deletions(-) diff --git a/OpenSim/Auxiliary/auxiliaryTestFunctions.h b/OpenSim/Auxiliary/auxiliaryTestFunctions.h index df97fdda6c..537bf70182 100644 --- a/OpenSim/Auxiliary/auxiliaryTestFunctions.h +++ b/OpenSim/Auxiliary/auxiliaryTestFunctions.h @@ -26,7 +26,9 @@ #include #include #include +#include #include +#include #include "getRSS.h" #include @@ -241,6 +243,7 @@ OpenSim::Object* randomize(OpenSim::Object* obj) // value. for (int p=0; p < obj->getNumProperties(); ++p) { AbstractProperty& ap = obj->updPropertyByIndex(p); + std::cout << obj->getName() << " " << ap.getName() << std::endl; //cout << ap.getName() << "=" << ap.toString() << endl; // Check return values from Property API for debugging purposes bool isList = ap.isListProperty(); @@ -249,7 +252,7 @@ OpenSim::Object* randomize(OpenSim::Object* obj) // bool t4 = ap.isOneValueProperty(); string ts = ap.getTypeName(); //cout << ts << endl; - if (ap.isOptionalProperty()) + if (ap.isOptionalProperty() && ts != "Function") continue; if (ts == "bool"&& !isList) ap.updValue() = (rand() % 2 == 0); @@ -283,19 +286,35 @@ OpenSim::Object* randomize(OpenSim::Object* obj) } else if (ts == "Function") { //FunctionSet's objects getTypeName() returns "Function" //which is wrong! This is a HACK to test that we aren't - //treating the PropertyObjArray as a Function. + //treating the PropertyObjArray as a Function PropertyObjArray* propObjArray = dynamic_cast*>(&ap); if (propObjArray){ if (propObjArray->size()){ randomize(&(propObjArray->updValueAsObject(0))); } - } - else{ - Property& prop = Property::updAs(ap); - LinearFunction f; - randomize(&f); - prop = f; + } else{ + if (isList) { + Property& prop = Property::updAs(ap); + for (int i=0; i< prop.size(); ++i) { + LinearFunction f; + randomize(&f); + prop.appendValue(f); + } + } else { + Property& prop = Property::updAs(ap); + // Special case for `FunctionBasedPath`. + if (prop.getName() == "lengthening_speed_function") { + MultivariatePolynomialFunction f( + createVector({1.0, 2.0, 3.0}), 2, 1); + randomize(&f); + prop = f; + } else { + LinearFunction f; + randomize(&f); + prop = f; + } + } } } else if (ap.isObjectProperty() && !isList) { diff --git a/OpenSim/Common/Object.h b/OpenSim/Common/Object.h index 4eee547100..879556e328 100644 --- a/OpenSim/Common/Object.h +++ b/OpenSim/Common/Object.h @@ -341,6 +341,9 @@ class OSIMCOMMON_API Object used by the Property declaration macros for fast access to properties. **/ template const Property& getProperty(const PropertyIndex& index) const; + /** @copydoc getProperty(const PropertyIndex&) **/ + template const Property& + getProperty(const std::string& name) const; /** Get property of known type Property\ as a writable reference; the property must be present and have the right type. This is primarily @@ -940,6 +943,11 @@ getProperty(const PropertyIndex& index) const { return _propertyTable.getProperty(index); } +template const Property& Object:: +getProperty(const std::string& name) const { + return _propertyTable.getProperty(name); +} + template Property& Object:: updProperty(const PropertyIndex& index) { _objectIsUpToDate = false; // property may be changed diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index ac80abfd55..9a0ae4ec7a 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -502,43 +502,55 @@ void OpenSim::appendCoupledCoordinateValues( } - -void OpenSim::computePathLengthsAndMomentArms( - const std::string& modelFile, - const std::string& coordinateValuesFile, - int threads = (int)std::thread::hardware_concurrency() - 2) { +TimeSeriesTable OpenSim::computePathLengthsAndMomentArms( + Model model, + const TimeSeriesTable& coordinateValues, + int threads, + double momentArmTolerance) { // Check inputs. OPENSIM_THROW_IF( - threads < 1 || threads > std::thread::hardware_concurrency(), + threads < 1 || threads > (int)std::thread::hardware_concurrency(), Exception, "Number of threads must be between 1 and {}.", std::thread::hardware_concurrency()); // Load model. - Model model(modelFile); + SimTK::State state = model.initSystem(); + + // Unlock all locked coordinates. + for (auto& coordinate : model.updComponentList()) { + coordinate.set_locked(false); + } + model.finalizeFromProperties(); model.initSystem(); // Load coordinate values. - TableProcessor tableProcessor = TableProcessor(coordinateValuesFile) | + TableProcessor tableProcessor = TableProcessor(coordinateValues) | TabOpUseAbsoluteStateNames() | TabOpAppendCoupledCoordinateValues(); - TimeSeriesTable coordinateValues = + TimeSeriesTable coordinateValuesProcessed = tableProcessor.processAndConvertToRadians(model); + + Array stateVariableNames = model.getStateVariableNames(); + for (const auto& label : coordinateValuesProcessed.getColumnLabels()) { + OPENSIM_THROW_IF(stateVariableNames.findIndex(label) == -1, Exception, + "Expected the model to contain the coordinate value state " + "'{}', but it does not.", label); + } auto statesTrajectory = StatesTrajectory::createFromStatesTable( - model, coordinateValues); + model, coordinateValuesProcessed, true); // Determine the maximum number of path and moment arm evaluations. const auto& paths = model.getComponentList(); int numPaths = (int)std::distance(paths.begin(), paths.end()); - int numCoordinates = (int)coordinateValues.getNumColumns(); - int numColumns = numCoordinates + (numPaths * numCoordinates); + int numCoordinates = (int)coordinateValuesProcessed.getNumColumns(); + int numColumns = numPaths + (numPaths * numCoordinates); // Define helper function for path length and moment arm computations. - auto calcPathLengthsAndMomentArmsSubset = [numColumns](Model model, - StatesTrajectory::IteratorRange subsetStates) -> SimTK::Matrix { - Logger::setLevel(Logger::Level::Error); + auto calcPathLengthsAndMomentArmsSubset = [numColumns, numPaths]( + Model model, StatesTrajectory::IteratorRange subsetStates) + -> SimTK::Matrix { model.initSystem(); - const CoordinateSet& coordinateSet = model.getCoordinateSet(); // Create a matrix to store the results SimTK::Matrix results( @@ -546,22 +558,38 @@ void OpenSim::computePathLengthsAndMomentArms( numColumns); int row = 0; + const auto& forces = model.getComponentList(); for (const auto& state : subsetStates) { model.realizePosition(state); - - - + + int ip = 0; + int ima = 0; + for (const auto& force : forces) { + if (force.hasProperty("path")) { + const AbstractPath& path = + force.getProperty("path").getValue(); + + // Compute path length. + results(row, ip++) = path.getLength(state); + + // Compute moment arms. + for (const auto& coordinate : + model.getComponentList()) { + results(row, numPaths + ima++) = + path.computeMomentArm(state, coordinate); + } + } + } row++; } return results; }; - // Divide the path length and moment arm computations across multiple // threads. int stride = static_cast( - std::floor(coordinateValues.getNumRows() / threads)); + std::floor(coordinateValuesProcessed.getNumRows() / threads)); std::vector> futures; int offset = 0; for (int ithread = 0; ithread < threads; ++ithread) { @@ -584,25 +612,49 @@ void OpenSim::computePathLengthsAndMomentArms( } // Assemble results into one TimeSeriesTable + TimeSeriesTable results; + std::vector time = coordinateValuesProcessed.getIndependentColumn(); + int itime = 0; + for (int i = 0; i < threads; ++i) { + for (int j = 0; j < outputs[i].nrow(); ++j) { + results.appendRow(time[itime++], outputs[i].row(j)); + } + } - // Gather data - - // Implement logic to combine data from multiple threads - - // Put data in a suitable data structure - - // Save data to file - - // Fit polynomial coefficients - - // You can implement this part based on your needs. - - std::cout << "Fit polynomials." << std::endl; - - // Save polynomialData to file + int ip = 0; + int ima = 0; + std::vector labels(numColumns); + const auto& forces = model.getComponentList(); + for (const auto& force : forces) { + if (force.hasProperty("path")) { + labels[ip++] = + fmt::format("{}_length", force.getAbsolutePathString()); + for (const auto& coordinate : + model.getComponentList()) { + labels[numPaths + ima++] = fmt::format("{}_moment_arm_{}", + force.getAbsolutePathString(), coordinate.getName()); + } + } + } + results.setColumnLabels(labels); + // Remove moment arm columns that contain values below a certain tolerance. + for (const auto& label : results.getColumnLabels()) { + if (label.find("_moment_arm_") != std::string::npos) { + const auto& col = results.getDependentColumn(label); + if (col.normInf() < momentArmTolerance) { + results.removeColumn(label); + } + } + } - std::cout << "Done fitting polynomials." << std::endl; + return results; } +//void OpenSim::fitFunctionBasedPathCoefficients() { +// +//} + + + diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 96bc662ad6..09ffea818b 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -387,10 +387,24 @@ OSIMSIMULATION_API TimeSeriesTableVec3 createSyntheticIMUAccelerationSignals( OSIMSIMULATION_API void appendCoupledCoordinateValues( TimeSeriesTable& table, const Model& model); -OSIMSIMULATION_API void computePathLengthsAndMomentArms( - const std::string& modelFile, - const std::string& coordinateValuesFile, - int threads); + +// TODO: 1) will unlock locked coordinates (so must provide data for these +// coordinates) +// 2) coordinate values must have number of columns equal to +// number of independent coordinates (Appends coupled coordinate +// values, if missing) +// 3) Updates the coordinates table to use absolute state names +OSIMSIMULATION_API TimeSeriesTable computePathLengthsAndMomentArms( + Model model, + const TimeSeriesTable& coordinateValues, + int threads = (int)std::thread::hardware_concurrency() - 2, + double momentArmTolerance = 1e-4); + +// TODO: 1) expects length and moment arm column names in specific format +//OSIMSIMULATION_API void fitFunctionBasedPathCoefficients( +// Model model, +// const TimeSeriesTable& pathLengthsAndMomentArms, +// ); } // end of namespace OpenSim From f23a75cac3525aa62ec000773d3a2fbb03e0633e Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 25 Sep 2023 10:00:38 -0700 Subject: [PATCH 05/34] First pass at polynomial fitting utility --- OpenSim/Actuators/ModelFactory.cpp | 25 ++ OpenSim/Actuators/ModelFactory.h | 33 +-- OpenSim/Auxiliary/auxiliaryTestFunctions.h | 31 +-- .../Common/MultivariatePolynomialFunction.cpp | 83 ++++-- .../Common/MultivariatePolynomialFunction.h | 4 + OpenSim/Simulation/SimulationUtilities.cpp | 256 +++++++++++++++--- OpenSim/Simulation/SimulationUtilities.h | 17 +- 7 files changed, 331 insertions(+), 118 deletions(-) diff --git a/OpenSim/Actuators/ModelFactory.cpp b/OpenSim/Actuators/ModelFactory.cpp index c61d6f6c5b..c8285ac516 100644 --- a/OpenSim/Actuators/ModelFactory.cpp +++ b/OpenSim/Actuators/ModelFactory.cpp @@ -308,3 +308,28 @@ void ModelFactory::createReserveActuators(Model& model, double optimalForce, log_info(" {}", name); } } + +void ModelFactory::replacePathsWithFunctionBasedPaths(OpenSim::Model& model, + const std::string& pathsFileName) { + Set pathSet(pathsFileName); + for (int i = 0; i < pathSet.getSize(); ++i) { + auto path = pathSet.get(i); + + // Get the force component associated with this path. + OPENSIM_THROW_IF(!model.hasComponent(path.getName()), + Exception, "Model does not contain a Force at path {}.", + path.getName()); + auto& force = model.updComponent(path.getName()); + + // Check that the force has a path property. + OPENSIM_THROW_IF( + !force.hasProperty("path"), Exception, + "Force {} does not have a path property.", path.getName()); + + // Update the path. + path.setName(fmt::format("{}_path", force.getName())); + force.updProperty("path").setValue(path); + } + model.finalizeFromProperties(); + model.finalizeConnections(); +} diff --git a/OpenSim/Actuators/ModelFactory.h b/OpenSim/Actuators/ModelFactory.h index 1c2b6b57b2..f26ed2d1dc 100644 --- a/OpenSim/Actuators/ModelFactory.h +++ b/OpenSim/Actuators/ModelFactory.h @@ -93,34 +93,13 @@ class OSIMACTUATORS_API ModelFactory { bool skipCoordinatesWithExistingActuators = true); /// Replace the paths of the forces in the model with the - /// FunctionBasedPath%s specified in the file 'pathsFileName'. The file must - /// be a Set of FunctionBasedPath%s where the name of each path matches the - /// path of a corresponding Force in the model. The path name is appended - /// with "_path" to avoid name ambiguity in the final model. + /// FunctionBasedPath%s specified in the file 'functionBasedPathsFile'. + /// The file must contain a Set of FunctionBasedPath%s where the name of + /// each path matches the path of a corresponding Force in the model. The + /// path name is appended with "_path" to avoid name ambiguity in the final + /// model. static void replacePathsWithFunctionBasedPaths(Model& model, - const std::string& pathsFileName) { - Set pathSet(pathsFileName); - for (int i = 0; i < pathSet.getSize(); ++i) { - auto path = pathSet.get(i); - - // Get the force component associated with this path. - OPENSIM_THROW_IF(!model.hasComponent(path.getName()), - Exception, "Model does not contain a Force at path {}.", - path.getName()); - auto& force = model.updComponent(path.getName()); - - // Check that the force has a path property. - OPENSIM_THROW_IF( - !force.hasProperty("path"), Exception, - "Force {} does not have a path property.", path.getName()); - - // Update the path. - path.setName(fmt::format("{}_path", force.getName())); - force.updProperty("path").setValue(path); - } - model.finalizeFromProperties(); - model.finalizeConnections(); - } + const std::string& functionBasedPathsFile); }; } // namespace OpenSim diff --git a/OpenSim/Auxiliary/auxiliaryTestFunctions.h b/OpenSim/Auxiliary/auxiliaryTestFunctions.h index 537bf70182..52a44ab084 100644 --- a/OpenSim/Auxiliary/auxiliaryTestFunctions.h +++ b/OpenSim/Auxiliary/auxiliaryTestFunctions.h @@ -26,7 +26,6 @@ #include #include #include -#include #include #include #include "getRSS.h" @@ -252,7 +251,7 @@ OpenSim::Object* randomize(OpenSim::Object* obj) // bool t4 = ap.isOneValueProperty(); string ts = ap.getTypeName(); //cout << ts << endl; - if (ap.isOptionalProperty() && ts != "Function") + if (ap.isOptionalProperty()) continue; if (ts == "bool"&& !isList) ap.updValue() = (rand() % 2 == 0); @@ -293,28 +292,12 @@ OpenSim::Object* randomize(OpenSim::Object* obj) if (propObjArray->size()){ randomize(&(propObjArray->updValueAsObject(0))); } - } else{ - if (isList) { - Property& prop = Property::updAs(ap); - for (int i=0; i< prop.size(); ++i) { - LinearFunction f; - randomize(&f); - prop.appendValue(f); - } - } else { - Property& prop = Property::updAs(ap); - // Special case for `FunctionBasedPath`. - if (prop.getName() == "lengthening_speed_function") { - MultivariatePolynomialFunction f( - createVector({1.0, 2.0, 3.0}), 2, 1); - randomize(&f); - prop = f; - } else { - LinearFunction f; - randomize(&f); - prop = f; - } - } + } + else{ + Property& prop = Property::updAs(ap); + LinearFunction f; + randomize(&f); + prop = f; } } else if (ap.isObjectProperty() && !isList) { diff --git a/OpenSim/Common/MultivariatePolynomialFunction.cpp b/OpenSim/Common/MultivariatePolynomialFunction.cpp index 9d4e645fc0..bb10ac08c1 100644 --- a/OpenSim/Common/MultivariatePolynomialFunction.cpp +++ b/OpenSim/Common/MultivariatePolynomialFunction.cpp @@ -74,9 +74,12 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { "Expected {} coefficients but got {}.", coeff_nr, coefficients.size()); } - T calcValue(const SimTK::Vector& x) const override { + + + SimTK::Vector_ calcTermValues(const SimTK::Vector& x) const { std::array nq{{0, 0, 0, 0, 0, 0}}; - T value = static_cast(0); + SimTK::Vector_ values = static_cast>( + SimTK::Vector(coefficients.size(), 0.0)); int coeff_nr = 0; for (nq[0] = 0; nq[0] < order + 1; ++nq[0]) { int nq2_s; @@ -113,7 +116,7 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { for (int i = 0; i < dimension; ++i) { valueP *= std::pow(x[i], nq[i]); } - value += valueP * coefficients[coeff_nr]; + values[coeff_nr] = valueP; ++coeff_nr; } } @@ -121,12 +124,20 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { } } } - return value; + return values; } - T calcDerivative(const SimTK::Array_& derivComponent, - const SimTK::Vector& x) const override { + + T calcValue(const SimTK::Vector& x) const override { + SimTK::Vector_ values = calcTermValues(x); + return ~coefficients * values; + } + + SimTK::Vector_ calcTermDerivatives( + const SimTK::Array_& derivComponent, + const SimTK::Vector& x) const { std::array nq{{0, 0, 0, 0, 0, 0}}; - T value = static_cast(0); + SimTK::Vector_ derivatives = static_cast>( + SimTK::Vector(coefficients.size(), 0.0)); int nqNonNegative; int coeff_nr = 0; for (nq[0] = 0; nq[0] < order + 1; ++nq[0]) { @@ -163,57 +174,57 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { if (derivComponent[0] == 0) { nqNonNegative = nq[0] - 1; if (nqNonNegative < 0) nqNonNegative = 0; - T valueP = nq[0] * std::pow(x[0], nqNonNegative); + T derivP = nq[0] * std::pow(x[0], nqNonNegative); for (int i = 0; i < dimension; ++i) { if (i == derivComponent[0]) continue; - valueP *= std::pow(x[i], nq[i]); + derivP *= std::pow(x[i], nq[i]); } - value += valueP * coefficients[coeff_nr]; + derivatives[coeff_nr] = derivP; } else if (derivComponent[0] == 1) { nqNonNegative = nq[1] - 1; if (nqNonNegative < 0) nqNonNegative = 0; - T valueP = nq[1] * std::pow(x[1], nqNonNegative); + T derivP = nq[1] * std::pow(x[1], nqNonNegative); for (int i = 0; i < dimension; ++i) { if (i == derivComponent[0]) continue; - valueP *= std::pow(x[i], nq[i]); + derivP *= std::pow(x[i], nq[i]); } - value += valueP * coefficients[coeff_nr]; + derivatives[coeff_nr] = derivP; } else if (derivComponent[0] == 2) { nqNonNegative = nq[2] - 1; if (nqNonNegative < 0) nqNonNegative = 0; - T valueP = nq[2] * std::pow(x[2], nqNonNegative); + T derivP = nq[2] * std::pow(x[2], nqNonNegative); for (int i = 0; i < dimension; ++i) { if (i == derivComponent[0]) continue; - valueP *= std::pow(x[i], nq[i]); + derivP *= std::pow(x[i], nq[i]); } - value += valueP * coefficients[coeff_nr]; + derivatives[coeff_nr] = derivP; } else if (derivComponent[0] == 3) { nqNonNegative = nq[3] - 1; if (nqNonNegative < 0) nqNonNegative = 0; - T valueP = nq[3] * std::pow(x[3], nqNonNegative); + T derivP = nq[3] * std::pow(x[3], nqNonNegative); for (int i = 0; i < dimension; ++i) { if (i == derivComponent[0]) continue; - valueP *= std::pow(x[i], nq[i]); + derivP *= std::pow(x[i], nq[i]); } - value += valueP * coefficients[coeff_nr]; + derivatives[coeff_nr] = derivP; } else if (derivComponent[0] == 4) { nqNonNegative = nq[4] - 1; if (nqNonNegative < 0) nqNonNegative = 0; - T valueP = nq[4] * std::pow(x[4], nqNonNegative); + T derivP = nq[4] * std::pow(x[4], nqNonNegative); for (int i = 0; i < dimension; ++i) { if (i == derivComponent[0]) continue; - valueP *= std::pow(x[i], nq[i]); + derivP *= std::pow(x[i], nq[i]); } - value += valueP * coefficients[coeff_nr]; + derivatives[coeff_nr] = derivP; } else if (derivComponent[0] == 5) { nqNonNegative = nq[5] - 1; if (nqNonNegative < 0) nqNonNegative = 0; - T valueP = nq[5] * std::pow(x[5], nqNonNegative); + T derivP = nq[5] * std::pow(x[5], nqNonNegative); for (int i = 0; i < dimension; ++i) { if (i == derivComponent[0]) continue; - valueP *= std::pow(x[i], nq[i]); + derivP *= std::pow(x[i], nq[i]); } - value += valueP * coefficients[coeff_nr]; + derivatives[coeff_nr] = derivP; } ++coeff_nr; } @@ -222,8 +233,15 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { } } } - return value; + return derivatives; } + + T calcDerivative(const SimTK::Array_& derivComponent, + const SimTK::Vector& x) const override { + SimTK::Vector_ derivatives = calcTermDerivatives(derivComponent, x); + return ~coefficients * derivatives; + } + int getArgumentSize() const override { return dimension; } int getMaxDerivativeOrder() const override { return 1; } SimTKMultivariatePolynomial* clone() const override { @@ -244,3 +262,16 @@ SimTK::Function* MultivariatePolynomialFunction::createSimTKFunction() const { return new SimTKMultivariatePolynomial( get_coefficients(), get_dimension(), getOrder()); } + +SimTK::Vector MultivariatePolynomialFunction::getTermValues( + const SimTK::Vector& x) const { + return static_cast*>( + _function)->calcTermValues(x); +} + +SimTK::Vector MultivariatePolynomialFunction::getTermDerivatives( + const std::vector& derivComponent, const SimTK::Vector& x) const { + return static_cast*>( + _function)->calcTermDerivatives( + SimTK::ArrayViewConst_(derivComponent), x); +} diff --git a/OpenSim/Common/MultivariatePolynomialFunction.h b/OpenSim/Common/MultivariatePolynomialFunction.h index 91908e49f5..8a26ed9b85 100644 --- a/OpenSim/Common/MultivariatePolynomialFunction.h +++ b/OpenSim/Common/MultivariatePolynomialFunction.h @@ -97,6 +97,10 @@ class OSIMCOMMON_API MultivariatePolynomialFunction : public Function { /// Return function SimTK::Function* createSimTKFunction() const override; + + SimTK::Vector getTermValues(const SimTK::Vector& x) const; + SimTK::Vector getTermDerivatives(const std::vector& derivComponent, + const SimTK::Vector& x) const; private: void constructProperties() { diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 9a0ae4ec7a..03e8b327c3 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -25,15 +25,17 @@ #include "Manager/Manager.h" #include "Model/Model.h" +#include -#include #include +#include + #include #include #include +#include -#include using namespace OpenSim; @@ -502,9 +504,12 @@ void OpenSim::appendCoupledCoordinateValues( } -TimeSeriesTable OpenSim::computePathLengthsAndMomentArms( +void OpenSim::computePathLengthsAndMomentArms( Model model, const TimeSeriesTable& coordinateValues, + TimeSeriesTable& pathLengths, + TimeSeriesTable& momentArms, + std::map>& momentArmMap, int threads, double momentArmTolerance) { @@ -513,37 +518,36 @@ TimeSeriesTable OpenSim::computePathLengthsAndMomentArms( threads < 1 || threads > (int)std::thread::hardware_concurrency(), Exception, "Number of threads must be between 1 and {}.", std::thread::hardware_concurrency()); + OPENSIM_THROW_IF(pathLengths.getNumRows() != 0, Exception, + "Expected 'pathLengths' to be empty."); + OPENSIM_THROW_IF(momentArms.getNumRows() != 0, Exception, + "Expected 'momentArms' to be empty."); + momentArmMap.clear(); // Load model. SimTK::State state = model.initSystem(); - // Unlock all locked coordinates. - for (auto& coordinate : model.updComponentList()) { - coordinate.set_locked(false); - } - model.finalizeFromProperties(); - model.initSystem(); - // Load coordinate values. - TableProcessor tableProcessor = TableProcessor(coordinateValues) | - TabOpUseAbsoluteStateNames() | - TabOpAppendCoupledCoordinateValues(); - TimeSeriesTable coordinateValuesProcessed = - tableProcessor.processAndConvertToRadians(model); + // TODO check coordinate values +// TableProcessor tableProcessor = TableProcessor(coordinateValues) | +// TabOpUseAbsoluteStateNames() | +// TabOpAppendCoupledCoordinateValues(); +// TimeSeriesTable coordinateValuesProcessed = +// tableProcessor.processAndConvertToRadians(model); Array stateVariableNames = model.getStateVariableNames(); - for (const auto& label : coordinateValuesProcessed.getColumnLabels()) { + for (const auto& label : coordinateValues.getColumnLabels()) { OPENSIM_THROW_IF(stateVariableNames.findIndex(label) == -1, Exception, "Expected the model to contain the coordinate value state " "'{}', but it does not.", label); } auto statesTrajectory = StatesTrajectory::createFromStatesTable( - model, coordinateValuesProcessed, true); + model, coordinateValues, true); // Determine the maximum number of path and moment arm evaluations. const auto& paths = model.getComponentList(); int numPaths = (int)std::distance(paths.begin(), paths.end()); - int numCoordinates = (int)coordinateValuesProcessed.getNumColumns(); + int numCoordinates = (int)coordinateValues.getNumColumns(); int numColumns = numPaths + (numPaths * numCoordinates); // Define helper function for path length and moment arm computations. @@ -589,7 +593,7 @@ TimeSeriesTable OpenSim::computePathLengthsAndMomentArms( // Divide the path length and moment arm computations across multiple // threads. int stride = static_cast( - std::floor(coordinateValuesProcessed.getNumRows() / threads)); + std::floor(coordinateValues.getNumRows() / threads)); std::vector> futures; int offset = 0; for (int ithread = 0; ithread < threads; ++ithread) { @@ -612,48 +616,230 @@ TimeSeriesTable OpenSim::computePathLengthsAndMomentArms( } // Assemble results into one TimeSeriesTable - TimeSeriesTable results; - std::vector time = coordinateValuesProcessed.getIndependentColumn(); + std::vector time = coordinateValues.getIndependentColumn(); int itime = 0; for (int i = 0; i < threads; ++i) { for (int j = 0; j < outputs[i].nrow(); ++j) { - results.appendRow(time[itime++], outputs[i].row(j)); + pathLengths.appendRow(time[itime], outputs[i].block(j, 0, 1, + numPaths).getAsRowVector()); + momentArms.appendRow(time[itime], outputs[i].block(j, numPaths, 1, + numPaths * numCoordinates).getAsRowVector()); + itime++; } } int ip = 0; int ima = 0; - std::vector labels(numColumns); + std::vector pathLengthLabels(numPaths); + std::vector momentArmLabels(numPaths * numCoordinates); const auto& forces = model.getComponentList(); for (const auto& force : forces) { if (force.hasProperty("path")) { - labels[ip++] = + pathLengthLabels[ip++] = fmt::format("{}_length", force.getAbsolutePathString()); for (const auto& coordinate : model.getComponentList()) { - labels[numPaths + ima++] = fmt::format("{}_moment_arm_{}", + momentArmLabels[ima++] = fmt::format("{}_moment_arm_{}", force.getAbsolutePathString(), coordinate.getName()); } } } - results.setColumnLabels(labels); + pathLengths.setColumnLabels(pathLengthLabels); + momentArms.setColumnLabels(momentArmLabels); - // Remove moment arm columns that contain values below a certain tolerance. - for (const auto& label : results.getColumnLabels()) { + // Remove columns for coupled coordinates. + for (const auto& couplerConstraint : + model.getComponentList()) { + auto momentArmLabel = fmt::format("_moment_arm_{}", + couplerConstraint.getDependentCoordinateName()); + for (const auto& label : momentArms.getColumnLabels()) { + if (label.find(momentArmLabel) != std::string::npos) { + momentArms.removeColumn(label); + } + } + } + + // Remove moment arm columns that contain values below the specified + // moment arm tolerance. + for (const auto& label : momentArms.getColumnLabels()) { if (label.find("_moment_arm_") != std::string::npos) { - const auto& col = results.getDependentColumn(label); + const auto& col = momentArms.getDependentColumn(label); if (col.normInf() < momentArmTolerance) { - results.removeColumn(label); + momentArms.removeColumn(label); + } else { + std::string path = label.substr(0, label.find("_moment_arm_")); + std::string coordinate = label.substr( + label.find("_moment_arm_") + 12); + momentArmMap[path].push_back(coordinate); } } } - - return results; } -//void OpenSim::fitFunctionBasedPathCoefficients() { -// -//} +void OpenSim::fitFunctionBasedPathCoefficients( + Model model, + const TimeSeriesTable& coordinateValues, + const TimeSeriesTable& pathLengths, + const TimeSeriesTable& momentArms, + const std::map>& momentArmMap) { + + + // Helper functions. + // ----------------- + // Factorial function. + auto factorial = [](int n) { + int result = 1; + for (int i = 1; i <= n; ++i) { + result *= i; + } + return result; + }; + + // Initialize model. + // ----------------- + model.initSystem(); + + // Coordinate references. + // ---------------------- + const CoordinateSet& coordinateSet = model.getCoordinateSet(); + const int numCoordinates = coordinateSet.getSize(); + // Map from coordinate name to index. + std::map coordinateIndexMap; + for (int i = 0; i < numCoordinates; ++i) { + coordinateIndexMap[coordinateSet[i].getName()] = i; + } + // Map from coordinate name to absolute path. + std::map coordinatePathMap; + for (int i = 0; i < numCoordinates; ++i) { + coordinatePathMap[coordinateSet[i].getName()] = + coordinateSet[i].getAbsolutePathString(); + } + // Number of time points. + const int numTimes = (int)coordinateValues.getNumRows(); + + // Maximum polynomial order. + const int maxOrder = 6; + + // Loop through model forces and compute function-based path coefficients. + for (const auto& force : model.getComponentList()) { + std::cout << "force path: " << force.getAbsolutePathString() << std::endl; + + // Check if the current + if (momentArmMap.find(force.getAbsolutePathString()) == + momentArmMap.end()) { + continue; + } + + // The current force path and the number of coordinates it depends on. + const std::string& forcePath = force.getAbsolutePathString(); + std::vector coordinatesNamesThisForce = + momentArmMap.at(forcePath); + int numCoordinatesThisForce = (int)coordinatesNamesThisForce.size(); + + // The path lengths for this force. + const SimTK::VectorView pathLengthsThisForce = + pathLengths.getDependentColumn( + fmt::format("{}_length", forcePath)); + + // The moment arms and coordinate values for this force. + SimTK::Matrix momentArmsThisForce(numTimes, + numCoordinatesThisForce, 0.0); + SimTK::Matrix coordinatesThisForce(numTimes, numCoordinatesThisForce, + 0.0); + for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { + const std::string& coordinateName = coordinatesNamesThisForce[ic]; + const SimTK::VectorView momentArmsThisCoordinate = + momentArms.getDependentColumn( + fmt::format("{}_moment_arm_{}", forcePath, + coordinateName)); + std::cout << "coordinate name: " << coordinateName << std::endl; + std::cout << "coordinate path: " << coordinatePathMap.at( + coordinateName) << std::endl; + const SimTK::VectorView coordinateValuesThisCoordinate = + coordinateValues.getDependentColumn( + fmt::format("{}/value", + coordinatePathMap.at(coordinateName))); + for (int itime = 0; itime < numTimes; ++itime) { + momentArmsThisForce.set( + itime, ic, momentArmsThisCoordinate[itime]); + coordinatesThisForce.set( + itime, ic, coordinateValuesThisCoordinate[itime]); + } + } + + // Polynomial fitting + bool foundFunctionBasedFit = false; + int order = 2; + while (order < maxOrder) { + + // Create a multivariate polynomial function. + int n = numCoordinatesThisForce + order; + int k = order; + int numCoefficients = + factorial(n) / (factorial(k) * factorial(n - k)); + SimTK::Vector dummyCoefficients(numCoefficients, 0.0); + MultivariatePolynomialFunction function( + dummyCoefficients, numCoordinatesThisForce, order); + + // TODO avoid this hack + std::cout << function.calcValue(coordinatesThisForce.row(0).getAsVector()) << std::endl; + + // Initialize A and b matrices. + SimTK::Matrix A(numTimes * (numCoordinatesThisForce + 1), + numCoefficients, 0.0); + SimTK::Vector b(numTimes * (numCoordinatesThisForce + 1), 0.0); + + // Fill in the A matrix. This contains the polynomial terms for the + // path length and moment arms. + for (int itime = 0; itime < numTimes; ++itime) { + A(itime, 0, 1, numCoefficients) = function.getTermValues( + coordinatesThisForce.row(itime).getAsVector()); + + for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { + SimTK::Vector termDerivatives = + function.getTermDerivatives({ic}, + coordinatesThisForce.row(itime).getAsVector()); + termDerivatives.negate(); + A((ic+1)*numTimes + itime, 0, 1, numCoefficients) = + function.getTermDerivatives({ic}, + coordinatesThisForce.row(itime).getAsVector()); + } + } + + // Fill in the b vector. This contains the path lengths and + // moment arms data. + b(0, numTimes) = pathLengthsThisForce; + for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { + b((ic+1)*numTimes, numTimes) = momentArmsThisForce.col(ic); + } + + // Solve the least-squares problem. + // A * x = b + // x = pinv(A)*b + // where pinv(A) = ~A / (A * ~A) + SimTK::Matrix pinv_A = (~A * A).invert() * ~A; + SimTK::Vector x_hat = pinv_A * b; + + SimTK::Vector b_hat = A * x_hat; + SimTK::Vector error = b - b_hat; + + std::cout << "order: " << order << std::endl; + std::cout << "coefficients: " << x_hat << std::endl; + + // Calculate RMS error from 'error' + double rmsError = std::sqrt((error.normSqr() / error.size())); + std::cout << "rms error: " << rmsError << std::endl; + + + + order++; + } + + + + } + +} diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 09ffea818b..2da0cfa37b 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -394,17 +394,22 @@ OSIMSIMULATION_API void appendCoupledCoordinateValues( // number of independent coordinates (Appends coupled coordinate // values, if missing) // 3) Updates the coordinates table to use absolute state names -OSIMSIMULATION_API TimeSeriesTable computePathLengthsAndMomentArms( +OSIMSIMULATION_API void computePathLengthsAndMomentArms( Model model, const TimeSeriesTable& coordinateValues, + TimeSeriesTable& pathLengths, + TimeSeriesTable& momentArms, + std::map>& momentArmMap, int threads = (int)std::thread::hardware_concurrency() - 2, - double momentArmTolerance = 1e-4); + double momentArmTolerance = 1e-3); // TODO: 1) expects length and moment arm column names in specific format -//OSIMSIMULATION_API void fitFunctionBasedPathCoefficients( -// Model model, -// const TimeSeriesTable& pathLengthsAndMomentArms, -// ); +OSIMSIMULATION_API void fitFunctionBasedPathCoefficients( + Model model, + const TimeSeriesTable& coordinateValues, + const TimeSeriesTable& pathLengths, + const TimeSeriesTable& momentArms, + const std::map>& momentArmMap); } // end of namespace OpenSim From 5c7d50559a6a7299eb00f1e8d87fcb623b57a1a3 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 25 Sep 2023 15:54:34 -0700 Subject: [PATCH 06/34] Testing the fitting utility --- OpenSim/Actuators/ModelFactory.cpp | 4 +- .../Common/MultivariatePolynomialFunction.cpp | 16 ++- .../Common/MultivariatePolynomialFunction.h | 3 +- .../Simulation/Model/FunctionBasedPath.cpp | 11 +- OpenSim/Simulation/SimulationUtilities.cpp | 136 ++++++++++-------- OpenSim/Simulation/SimulationUtilities.h | 6 +- 6 files changed, 100 insertions(+), 76 deletions(-) diff --git a/OpenSim/Actuators/ModelFactory.cpp b/OpenSim/Actuators/ModelFactory.cpp index c8285ac516..b8e2bd0cf8 100644 --- a/OpenSim/Actuators/ModelFactory.cpp +++ b/OpenSim/Actuators/ModelFactory.cpp @@ -310,8 +310,8 @@ void ModelFactory::createReserveActuators(Model& model, double optimalForce, } void ModelFactory::replacePathsWithFunctionBasedPaths(OpenSim::Model& model, - const std::string& pathsFileName) { - Set pathSet(pathsFileName); + const std::string& functionBasedPathsFile) { + Set pathSet(functionBasedPathsFile); for (int i = 0; i < pathSet.getSize(); ++i) { auto path = pathSet.get(i); diff --git a/OpenSim/Common/MultivariatePolynomialFunction.cpp b/OpenSim/Common/MultivariatePolynomialFunction.cpp index bb10ac08c1..20192e1601 100644 --- a/OpenSim/Common/MultivariatePolynomialFunction.cpp +++ b/OpenSim/Common/MultivariatePolynomialFunction.cpp @@ -70,11 +70,11 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { } } } + OPENSIM_THROW_IF(coefficients.size() != coeff_nr, Exception, "Expected {} coefficients but got {}.", coeff_nr, coefficients.size()); } - SimTK::Vector_ calcTermValues(const SimTK::Vector& x) const { std::array nq{{0, 0, 0, 0, 0, 0}}; @@ -127,11 +127,6 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { return values; } - T calcValue(const SimTK::Vector& x) const override { - SimTK::Vector_ values = calcTermValues(x); - return ~coefficients * values; - } - SimTK::Vector_ calcTermDerivatives( const SimTK::Array_& derivComponent, const SimTK::Vector& x) const { @@ -236,6 +231,11 @@ class SimTKMultivariatePolynomial : public SimTK::Function_ { return derivatives; } + T calcValue(const SimTK::Vector& x) const override { + SimTK::Vector_ values = calcTermValues(x); + return ~coefficients * values; + } + T calcDerivative(const SimTK::Array_& derivComponent, const SimTK::Vector& x) const override { SimTK::Vector_ derivatives = calcTermDerivatives(derivComponent, x); @@ -265,12 +265,16 @@ SimTK::Function* MultivariatePolynomialFunction::createSimTKFunction() const { SimTK::Vector MultivariatePolynomialFunction::getTermValues( const SimTK::Vector& x) const { + if (_function == NULL) + _function = createSimTKFunction(); return static_cast*>( _function)->calcTermValues(x); } SimTK::Vector MultivariatePolynomialFunction::getTermDerivatives( const std::vector& derivComponent, const SimTK::Vector& x) const { + if (_function == NULL) + _function = createSimTKFunction(); return static_cast*>( _function)->calcTermDerivatives( SimTK::ArrayViewConst_(derivComponent), x); diff --git a/OpenSim/Common/MultivariatePolynomialFunction.h b/OpenSim/Common/MultivariatePolynomialFunction.h index 8a26ed9b85..dc77e4155b 100644 --- a/OpenSim/Common/MultivariatePolynomialFunction.h +++ b/OpenSim/Common/MultivariatePolynomialFunction.h @@ -53,7 +53,8 @@ Index | X Y Z 18 | 2 1 0 19 | 3 0 0 -Assuming c6 the index 6 coefficient, the corresponding term is Y Z^2. +Assuming c6 the index 6 coefficient, the correspo + nding term is Y Z^2. @note The order of coefficients for this class is the *opposite** from the order used in the univariate PolynomialFunction. @param dimension the number of independent components diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index 58153858db..17f00a3499 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -125,9 +125,14 @@ double FunctionBasedPath::getLength(const SimTK::State& s) const double FunctionBasedPath::computeMomentArm(const SimTK::State& s, const Coordinate& coord) const { - computeMomentArms(s); - return getCacheVariableValue(s, MOMENT_ARMS_NAME) - .get(_coordinateIndices.at(coord.getAbsolutePathString())); + if (_coordinateIndices.find(coord.getAbsolutePathString()) != + _coordinateIndices.end()) { + computeMomentArms(s); + return getCacheVariableValue(s, MOMENT_ARMS_NAME) + .get(_coordinateIndices.at(coord.getAbsolutePathString())); + } else { + return 0.0; + } } double FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 03e8b327c3..f7749b766d 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -26,16 +26,15 @@ #include "Manager/Manager.h" #include "Model/Model.h" #include - #include #include -#include -#include -#include #include +#include +#include +#include using namespace OpenSim; @@ -676,13 +675,13 @@ void OpenSim::computePathLengthsAndMomentArms( } } -void OpenSim::fitFunctionBasedPathCoefficients( +double OpenSim::fitFunctionBasedPathCoefficients( Model model, const TimeSeriesTable& coordinateValues, const TimeSeriesTable& pathLengths, const TimeSeriesTable& momentArms, - const std::map>& momentArmMap) { - + const std::map>& momentArmMap, + const int minOrder, const int maxOrder) { // Helper functions. // ----------------- @@ -703,30 +702,23 @@ void OpenSim::fitFunctionBasedPathCoefficients( // ---------------------- const CoordinateSet& coordinateSet = model.getCoordinateSet(); const int numCoordinates = coordinateSet.getSize(); - // Map from coordinate name to index. - std::map coordinateIndexMap; - for (int i = 0; i < numCoordinates; ++i) { - coordinateIndexMap[coordinateSet[i].getName()] = i; - } - // Map from coordinate name to absolute path. - std::map coordinatePathMap; - for (int i = 0; i < numCoordinates; ++i) { - coordinatePathMap[coordinateSet[i].getName()] = - coordinateSet[i].getAbsolutePathString(); - } - // Number of time points. const int numTimes = (int)coordinateValues.getNumRows(); - // Maximum polynomial order. - const int maxOrder = 6; - - // Loop through model forces and compute function-based path coefficients. + // Build a FunctionBasedPath for each path-based force in the model. + // ----------------------------------------------------------------- + Set functionBasedPaths; + const auto forces = model.getComponentList(); + const int numForces = (int)std::distance(forces.begin(), forces.end()); + SimTK::Vector bestRootMeanSquareErrors(numForces, SimTK::Infinity); + int iforce = 0; for (const auto& force : model.getComponentList()) { - std::cout << "force path: " << force.getAbsolutePathString() << std::endl; - // Check if the current + // Check if the current force is dependent on any coordinates in the + // model. If not, skip it. if (momentArmMap.find(force.getAbsolutePathString()) == momentArmMap.end()) { + bestRootMeanSquareErrors[iforce] = 0.0; + ++iforce; continue; } @@ -735,6 +727,11 @@ void OpenSim::fitFunctionBasedPathCoefficients( std::vector coordinatesNamesThisForce = momentArmMap.at(forcePath); int numCoordinatesThisForce = (int)coordinatesNamesThisForce.size(); + std::vector coordinatePathsThisForce; + for (const auto& coordinateName : coordinatesNamesThisForce) { + coordinatePathsThisForce.push_back( + coordinateSet.get(coordinateName).getAbsolutePathString()); + } // The path lengths for this force. const SimTK::VectorView pathLengthsThisForce = @@ -752,13 +749,9 @@ void OpenSim::fitFunctionBasedPathCoefficients( momentArms.getDependentColumn( fmt::format("{}_moment_arm_{}", forcePath, coordinateName)); - std::cout << "coordinate name: " << coordinateName << std::endl; - std::cout << "coordinate path: " << coordinatePathMap.at( - coordinateName) << std::endl; const SimTK::VectorView coordinateValuesThisCoordinate = - coordinateValues.getDependentColumn( - fmt::format("{}/value", - coordinatePathMap.at(coordinateName))); + coordinateValues.getDependentColumn(fmt::format("{}/value", + coordinatePathsThisForce[ic])); for (int itime = 0; itime < numTimes; ++itime) { momentArmsThisForce.set( itime, ic, momentArmsThisCoordinate[itime]); @@ -767,22 +760,21 @@ void OpenSim::fitFunctionBasedPathCoefficients( } } - // Polynomial fitting - bool foundFunctionBasedFit = false; - int order = 2; - while (order < maxOrder) { + // Polynomial fitting. + // ------------------- + double bestRootMeanSquareError = SimTK::Infinity; + SimTK::Vector bestCoefficients; + int bestOrder = minOrder; + for (int order = minOrder; order <= maxOrder; ++order) { - // Create a multivariate polynomial function. + // Initialize the multivariate polynomial function. int n = numCoordinatesThisForce + order; int k = order; int numCoefficients = factorial(n) / (factorial(k) * factorial(n - k)); SimTK::Vector dummyCoefficients(numCoefficients, 0.0); - MultivariatePolynomialFunction function( - dummyCoefficients, numCoordinatesThisForce, order); - - // TODO avoid this hack - std::cout << function.calcValue(coordinatesThisForce.row(0).getAsVector()) << std::endl; + MultivariatePolynomialFunction dummyFunction(dummyCoefficients, + numCoordinatesThisForce, order); // Initialize A and b matrices. SimTK::Matrix A(numTimes * (numCoordinatesThisForce + 1), @@ -792,17 +784,17 @@ void OpenSim::fitFunctionBasedPathCoefficients( // Fill in the A matrix. This contains the polynomial terms for the // path length and moment arms. for (int itime = 0; itime < numTimes; ++itime) { - A(itime, 0, 1, numCoefficients) = function.getTermValues( + A(itime, 0, 1, numCoefficients) = dummyFunction.getTermValues( coordinatesThisForce.row(itime).getAsVector()); for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { SimTK::Vector termDerivatives = - function.getTermDerivatives({ic}, - coordinatesThisForce.row(itime).getAsVector()); + dummyFunction.getTermDerivatives({ic}, + coordinatesThisForce.row(itime).getAsVector()); termDerivatives.negate(); A((ic+1)*numTimes + itime, 0, 1, numCoefficients) = - function.getTermDerivatives({ic}, - coordinatesThisForce.row(itime).getAsVector()); + dummyFunction.getTermDerivatives({ic}, + coordinatesThisForce.row(itime).getAsVector()); } } @@ -813,32 +805,52 @@ void OpenSim::fitFunctionBasedPathCoefficients( b((ic+1)*numTimes, numTimes) = momentArmsThisForce.col(ic); } - // Solve the least-squares problem. - // A * x = b - // x = pinv(A)*b - // where pinv(A) = ~A / (A * ~A) + // Solve the least-squares problem. A is a rectangular matrix with + // full column rank, so we can use the left pseudo-inverse to solve + // for the coefficients. SimTK::Matrix pinv_A = (~A * A).invert() * ~A; - SimTK::Vector x_hat = pinv_A * b; - - SimTK::Vector b_hat = A * x_hat; - SimTK::Vector error = b - b_hat; - - std::cout << "order: " << order << std::endl; - std::cout << "coefficients: " << x_hat << std::endl; - - // Calculate RMS error from 'error' - double rmsError = std::sqrt((error.normSqr() / error.size())); - std::cout << "rms error: " << rmsError << std::endl; + SimTK::Vector coefficients = pinv_A * b; + // Calculate the RMS error. + SimTK::Vector b_fit = A * coefficients; + SimTK::Vector error = b - b_fit; + const double rmsError = std::sqrt((error.normSqr() / error.size())); + // Save best solution. + if (rmsError < bestRootMeanSquareErrors[iforce]) { + bestRootMeanSquareErrors[iforce] = rmsError; + bestCoefficients = coefficients; + bestOrder = order; + } - order++; + ++order; } + // Create a FunctionBasedPath for the current path-based force. + MultivariatePolynomialFunction lengthFunction; + lengthFunction.setDimension(numCoordinatesThisForce); + lengthFunction.setOrder(bestOrder); + lengthFunction.setCoefficients(bestCoefficients); + auto functionBasedPath = std::make_unique(); + functionBasedPath->setName(forcePath); + functionBasedPath->setCoordinatePaths(coordinatePathsThisForce); + functionBasedPath->setLengthFunction(lengthFunction); + functionBasedPaths.adoptAndAppend(functionBasedPath.release()); + ++iforce; } + // Save the function-based paths to a file. + // ---------------------------------------- + functionBasedPaths.print("testPolynomialFitting_Set_FunctionBasedPaths.xml"); + + // Return the average RMS error. + // ----------------------------- + double averageRootMeanSquareError = bestRootMeanSquareErrors.sum() / + bestRootMeanSquareErrors.size(); + + return averageRootMeanSquareError; } diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 2da0cfa37b..6e23780762 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -404,12 +404,14 @@ OSIMSIMULATION_API void computePathLengthsAndMomentArms( double momentArmTolerance = 1e-3); // TODO: 1) expects length and moment arm column names in specific format -OSIMSIMULATION_API void fitFunctionBasedPathCoefficients( +// 2) returns average RMS error +OSIMSIMULATION_API double fitFunctionBasedPathCoefficients( Model model, const TimeSeriesTable& coordinateValues, const TimeSeriesTable& pathLengths, const TimeSeriesTable& momentArms, - const std::map>& momentArmMap); + const std::map>& momentArmMap, + const int minOrder = 2, const int maxOrder = 6); } // end of namespace OpenSim From 57ebe85e97ae613214f5238eade404b2dff4f70c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 26 Sep 2023 08:40:28 -0700 Subject: [PATCH 07/34] Fitting utility is working! --- OpenSim/Simulation/Model/FunctionBasedPath.cpp | 2 +- OpenSim/Simulation/SimulationUtilities.cpp | 8 +++----- OpenSim/Simulation/SimulationUtilities.h | 2 +- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index 17f00a3499..72faccc2a3 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -366,7 +366,7 @@ void FunctionBasedPath::extendAddToSystem( addCacheVariable(LENGTH_NAME, 0.0, SimTK::Stage::Position); addCacheVariable(MOMENT_ARMS_NAME, SimTK::Vector(getProperty_coordinate_paths().size(), 0.0), - SimTK::Stage::Velocity); + SimTK::Stage::Position); addCacheVariable(LENGTHENING_SPEED_NAME, 0.0, SimTK::Stage::Velocity); } diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index f7749b766d..b06ee029ab 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -541,7 +541,7 @@ void OpenSim::computePathLengthsAndMomentArms( "'{}', but it does not.", label); } auto statesTrajectory = StatesTrajectory::createFromStatesTable( - model, coordinateValues, true); + model, coordinateValues, true, false, false); // Determine the maximum number of path and moment arm evaluations. const auto& paths = model.getComponentList(); @@ -790,11 +790,9 @@ double OpenSim::fitFunctionBasedPathCoefficients( for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { SimTK::Vector termDerivatives = dummyFunction.getTermDerivatives({ic}, - coordinatesThisForce.row(itime).getAsVector()); - termDerivatives.negate(); + coordinatesThisForce.row(itime).getAsVector()).negate(); A((ic+1)*numTimes + itime, 0, 1, numCoefficients) = - dummyFunction.getTermDerivatives({ic}, - coordinatesThisForce.row(itime).getAsVector()); + termDerivatives; } } diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 6e23780762..5e23c156b8 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -411,7 +411,7 @@ OSIMSIMULATION_API double fitFunctionBasedPathCoefficients( const TimeSeriesTable& pathLengths, const TimeSeriesTable& momentArms, const std::map>& momentArmMap, - const int minOrder = 2, const int maxOrder = 6); + const int minOrder = 2, const int maxOrder = 9); } // end of namespace OpenSim From a33625982d6b411c59b927a5f10d1b385c4c3666 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 2 Oct 2023 10:54:18 -0700 Subject: [PATCH 08/34] Prototyping a class for generating Latin hypercube designs --- OpenSim/Common/CommonUtilities.cpp | 40 ++++ OpenSim/Common/CommonUtilities.h | 13 +- OpenSim/Common/LatinHypercubeDesign.cpp | 235 ++++++++++++++++++++++++ OpenSim/Common/LatinHypercubeDesign.h | 54 ++++++ OpenSim/Common/osimCommon.h | 1 + 5 files changed, 342 insertions(+), 1 deletion(-) create mode 100644 OpenSim/Common/LatinHypercubeDesign.cpp create mode 100644 OpenSim/Common/LatinHypercubeDesign.h diff --git a/OpenSim/Common/CommonUtilities.cpp b/OpenSim/Common/CommonUtilities.cpp index 4bdc4adf03..180bf88d14 100644 --- a/OpenSim/Common/CommonUtilities.cpp +++ b/OpenSim/Common/CommonUtilities.cpp @@ -183,3 +183,43 @@ SimTK::Real OpenSim::solveBisection( } return midpoint; } + +SimTK::Matrix OpenSim::computeKNearestNeighbors(const SimTK::Matrix& x, + const SimTK::Matrix& y, int k) { + + OPENSIM_THROW_IF(x.ncol() != y.ncol(), Exception, + "Expected the matrices 'x' and 'y' to have the same number of " + "columns, but found {} and {}, respectively.", x.ncol(), y.ncol()); + + // Initialize the output matrices. + SimTK::Matrix distances(y.nrow(), k, 0.0); + + // Loop over each row in 'y'. + for (int iy = 0; iy < y.nrow(); ++iy) { + std::vector distancesVec; + + // Compute distances between the current row in 'y' and all rows in 'x'. + // Skip the distance between the current row in 'y' and itself. + for (int ix = 0; ix < x.nrow(); ++ix) { + const double distance = (y.row(iy) - x.row(ix)).normSqr(); + distancesVec.push_back(distance); + } + + // Sort the distances in ascending order. + std::sort(distancesVec.begin(), distancesVec.end(), + [](const double& a, const double& b) { + return a < b; + }); + + // Take the first K distances. + for (int ik = 0; ik < k; ++ik) { + distances.set(iy, ik, distancesVec[ik]); + } + } + + return distances; +} + +// + + diff --git a/OpenSim/Common/CommonUtilities.h b/OpenSim/Common/CommonUtilities.h index aa79c1f2f3..7c465858b9 100644 --- a/OpenSim/Common/CommonUtilities.h +++ b/OpenSim/Common/CommonUtilities.h @@ -24,13 +24,14 @@ * -------------------------------------------------------------------------- */ #include "osimCommonDLL.h" +#include "Assertion.h" #include #include #include #include #include -#include +#include #include namespace OpenSim { @@ -165,6 +166,16 @@ template class ThreadsafeJar { std::condition_variable m_inventoryMonitor; }; +// A C++ function for finding the K nearest nearest neighbors of two +// matrices X and Y, where X is MX-by-N and Y is MY-by-N. Rows of X and Y +// correspond to observations and columns correspond to variables. The function +// should return a the MY-by-K matrices D and IDX. D sorts the distances in each +// row in ascending order. Each row in IDX contains the indices of K closest +// neighbors in X corresponding to the K smallest distances in D. +OSIMCOMMON_API SimTK::Matrix computeKNearestNeighbors(const SimTK::Matrix& x, + const SimTK::Matrix& y, int k = 1); + + } // namespace OpenSim #endif // OPENSIM_COMMONUTILITIES_H_ diff --git a/OpenSim/Common/LatinHypercubeDesign.cpp b/OpenSim/Common/LatinHypercubeDesign.cpp new file mode 100644 index 0000000000..7ff4de9c14 --- /dev/null +++ b/OpenSim/Common/LatinHypercubeDesign.cpp @@ -0,0 +1,235 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: LatinHypercubeDesign.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2023 Stanford University and the Authors * + * Author(s): Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "LatinHypercubeDesign.h" +#include + +#include +#include + +using namespace OpenSim; + +double LatinHypercubeDesign::computeMaximinDistance( + const SimTK::Matrix& x) { + SimTK::Matrix nearestNeighbors = computeKNearestNeighbors(x, x, 2); + return SimTK::min(nearestNeighbors.col(1)); +} + +double LatinHypercubeDesign::computePhiPDistanceCriterion( + const SimTK::Matrix& x, const SimTK::Matrix& distances, int p) { + + double sumInverseDistancesP = 0; + for (int i = 0; i < x.nrow(); ++i) { + for (int j = 0; j < x.nrow(); ++j) { + if (i < j) { + sumInverseDistancesP += 1.0 / pow(distances(i, j), p); + } + } + } + + return pow(sumInverseDistancesP, 1.0 / p); +} + +SimTK::Matrix LatinHypercubeDesign::computeIntersiteDistanceMatrix( + const SimTK::Matrix& x) { + + // Compute a matrix of distance values, where each element is the distance + // between the corresponding rows in 'x' and 'y'. The elements should be + // d_ij where 1 <= i, j <= n, i != j, where n is the number of rows in + // 'x' and 'y'. + SimTK::Matrix distances(x.nrow(), x.nrow(), 0.0); + for (int i = 0; i < x.nrow(); ++i) { + for (int j = 0; j < x.nrow(); ++j) { + if (i != j) { + distances(i, j) = (x.row(i) - x.row(j)).normSqr(); + } + } + } + + return distances; +} + +SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( + int numSamples, int numVariables, SimTK::Matrix seed, int numSeedPoints) +{ + // Sorting helper functions. + // ------------------------- + // https://stackoverflow.com/questions/1577475/c-sorting-and-keeping-track-of-indexes + auto sort_indexes = [](const std::vector& v) -> std::vector { + std::vector idx(v.size()); + std::iota(idx.begin(), idx.end(), 0); + std::stable_sort(idx.begin(), idx.end(), + [&v](size_t i1, size_t i2) {return v[i1] < v[i2];}); + return idx; + }; + + // Same as above, but for SimTK::Vector. + auto sort_indexes_simtk = [](const SimTK::Vector& v) -> std::vector { + std::vector idx(v.size()); + std::iota(idx.begin(), idx.end(), 0); + std::stable_sort(idx.begin(), idx.end(), + [&v](size_t i1, size_t i2) {return v[i1] < v[i2];}); + return idx; + }; + + // Define the size of the TPLHD to be created first. + // ------------------------------------------------ + double numDivisions = + std::pow(numSamples / numSeedPoints, 1.0 / numVariables); + int numDivisionsRounded = (int)std::ceil(numDivisions); + int numSeeds; + if (numDivisionsRounded > numDivisions) { + numSeeds = (int)std::pow(numDivisionsRounded, numVariables); + } else { + OPENSIM_ASSERT_ALWAYS(numSamples % numSeedPoints == 0); + numSeeds = numSamples / numSeedPoints; + } + int numSamplesRounded = numSeeds * numSeedPoints; + + // Reshape the seed to create the first design. + // -------------------------------------------- + if (numSeedPoints == 1) { + seed = SimTK::Matrix(1, numVariables, 1.0); + } else { + SimTK::RowVector uf(numVariables, numSeedPoints); + SimTK::RowVector ut(numVariables, + (numSamplesRounded / numDivisionsRounded) - + numDivisionsRounded * (numVariables - 1) + 1); + SimTK::RowVector a = (ut - 1).elementwiseDivide(uf - 1); + SimTK::RowVector b = ut - a.elementwiseMultiply(uf); + for (int i = 0; i < numSeedPoints; ++i) { + seed.updRow(i).elementwiseMultiplyInPlace(a); + seed.updRow(i) += b; + + // Round all the elements of the seed to the nearest integer. + for (int j = 0; j < numVariables; ++j) { + seed[i][j] = std::round(seed[i][j]); + } + } + } + + // Create the TPLHD. + // ----------------- + SimTK::Matrix tplhd(numSamplesRounded, numVariables); + + // Fill in the first seed. + for (int i = 0; i < numSeedPoints; ++i) { + tplhd.updRow(i) = seed.row(i); + } + + SimTK::RowVector seedUpdate(numVariables, 1.0); + for (int i = 0; i < numVariables; ++i) { + // Update the seed with the most recently added points. + seed = tplhd.block(0, 0, numSeedPoints, numVariables); + std::cout << "seed: " << seed << std::endl; + + // Prepare the seed update. + for (int j = 0; j < i; ++j) { + seedUpdate[j] = std::pow(numDivisionsRounded, i - 1); + } + seedUpdate[i] = numSamplesRounded / numDivisionsRounded; + for (int j = i + 1; j < numVariables; ++j) { + seedUpdate[j] = std::pow(numDivisionsRounded, i); + } + + // Fill in each of the divisions. + int numNewSeedPoints = 0; + for (int j = 1; j < numDivisionsRounded; ++j) { + + // Update the seed. + for (int k = 0; k < (int)seed.nrow(); ++k) { + seed.updRow(k) += seedUpdate; + } + + // Update the TPLHD. + tplhd.updBlock(numSeedPoints, 0, (int)seed.nrow(), numVariables) = + seed; + numSeedPoints += (int)seed.nrow(); + } + } + + std::cout << "tplhd: " << tplhd << std::endl; + + // If necessary, resize the TPLHD. + // ------------------------------- + if (numSamplesRounded > numSamples) { + SimTK::Matrix tplhdResized(numSamples, numVariables); + + // Center the design space. + SimTK::RowVector center(numVariables, 0.5*numSamplesRounded); + + // Compute the distance between each point and the center. + std::vector distance; + distance.reserve(numSamplesRounded); + for (int i = 0; i < numSamplesRounded; ++i) { + distance.push_back((tplhd.row(i) - center).norm()); + } + + // Resize the TPLHD by taking the 'numSamples' points closest to the + // center. + int isample = 0; + for (auto i : sort_indexes(distance)) { + if (isample >= numSamples) { + break; + } + tplhdResized.updRow(isample) = tplhd.row(i); + ++isample; + } + + // Re-establish the Latin hypercube conditions. + SimTK::RowVector tplhdMinimums = SimTK::min(tplhdResized); + for (int i = 0; i < numVariables; ++i) { + + // Place the current design at the origin. + std::vector sortedColumnIndexes = + sort_indexes_simtk(tplhdResized.col(i)); + SimTK::Matrix tplhdTemp = tplhdResized; + for (int j = 0; j < numSamples; ++j) { + tplhdResized.updRow(j) = tplhdTemp.row(sortedColumnIndexes[j]); + } + tplhdResized.updCol(i) -= tplhdMinimums[i]; + tplhdResized.updCol(i) += 1; + + // Eliminate empty coordinates. + bool flag = false; + while (!flag) { + std::vector mask; + mask.reserve(numSamples); + SimTK::Vector maskVec(numSamples, 0.0); + for (int j = 0; j < numSamples; ++j) { + mask.push_back(tplhdResized(j, i) != j+1); + maskVec[j] = mask.back(); + } + + // If all elements in mask are false, then set flag to true. + flag = std::all_of(mask.begin(), mask.end(), + [](bool b) {return !b;}); + + tplhdResized.updCol(i) -= maskVec; + } + } + return tplhdResized; + } else { + return tplhd; + } +} \ No newline at end of file diff --git a/OpenSim/Common/LatinHypercubeDesign.h b/OpenSim/Common/LatinHypercubeDesign.h new file mode 100644 index 0000000000..2421bade9e --- /dev/null +++ b/OpenSim/Common/LatinHypercubeDesign.h @@ -0,0 +1,54 @@ +#ifndef OPENSIM_LATINHYPERCUBEDESIGN_H +#define OPENSIM_LATINHYPERCUBEDESIGN_H +/* -------------------------------------------------------------------------- * + * OpenSim: LatinHypercubeDesign.h * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2023 Stanford University and the Authors * + * Author(s): Nicholas Bianco * + * * + * Licensed under the Apache License, Version 2.0 (the "License"); you may * + * not use this file except in compliance with the License. You may obtain a * + * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * + * * + * Unless required by applicable law or agreed to in writing, software * + * distributed under the License is distributed on an "AS IS" BASIS, * + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * + * See the License for the specific language governing permissions and * + * limitations under the License. * + * -------------------------------------------------------------------------- */ + +#include "osimCommonDLL.h" +#include "CommonUtilities.h" + +namespace OpenSim { + +// A class for generating Latin hypercube designs. +class OSIMCOMMON_API LatinHypercubeDesign { +public: + LatinHypercubeDesign() = default; + + SimTK::Matrix computeTranslationalPropagationDesign(int numSamples, + int numVariables, SimTK::Matrix seed, int numSeedPoints); + +private: + std::string m_criterion = "maximin"; // maximin, phi_p + + double computeMaximinDistance(const SimTK::Matrix& x); + + SimTK::Matrix computeIntersiteDistanceMatrix( + const SimTK::Matrix& x); + + double computePhiPDistanceCriterion(const SimTK::Matrix& x, + const SimTK::Matrix& distances, int p = 50); + +}; + +} // namespace OpenSim + +#endif // OPENSIM_LATINHYPERCUBEDESIGN_H diff --git a/OpenSim/Common/osimCommon.h b/OpenSim/Common/osimCommon.h index 1c79481406..23cd153237 100644 --- a/OpenSim/Common/osimCommon.h +++ b/OpenSim/Common/osimCommon.h @@ -33,6 +33,7 @@ #include "GCVSpline.h" #include "GCVSplineSet.h" #include "IO.h" +#include "LatinHypercubeDesign.h" #include "LinearFunction.h" #include "LoadOpenSimLibrary.h" #include "Logger.h" From d90672e8fb88d8eda0fa9f5d59f35fb65506af33 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 2 Oct 2023 10:56:44 -0700 Subject: [PATCH 09/34] Re-arranging the new utilities --- .../Simulation/Model/FunctionBasedPath.cpp | 1 + OpenSim/Simulation/SimulationUtilities.cpp | 316 +++++++++++------- OpenSim/Simulation/SimulationUtilities.h | 53 ++- OpenSim/Simulation/TableProcessor.h | 19 +- 4 files changed, 251 insertions(+), 138 deletions(-) diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index 3021685c86..b14de93cef 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -47,6 +47,7 @@ void FunctionBasedPath::appendCoordinatePath(const std::string& coordinatePath) void FunctionBasedPath::setCoordinatePaths( const std::vector& coordinatePaths) { + updProperty_coordinate_paths().clear(); for (const auto& coordinatePath : coordinatePaths) { appendCoordinatePath(coordinatePath); } diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index b06ee029ab..4c855e7715 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include @@ -122,7 +123,7 @@ OpenSim::updatePre40KinematicsStorageFor40MotionType(const Model& pre40Model, // There is no issue if the kinematics are in internal values (i.e. not // converted to degrees) if(!kinematics.isInDegrees()) return nullptr; - + if (pre40Model.getDocumentFileVersion() >= 30415) { throw Exception("updateKinematicsStorageForUpdatedModel has no updates " "to make because the model '" + pre40Model.getName() + "'is up-to-date.\n" @@ -130,30 +131,30 @@ OpenSim::updatePre40KinematicsStorageFor40MotionType(const Model& pre40Model, "nothing further must be done. Otherwise, provide the original model " "file used to generate the motion files and try again."); } - + std::vector problemCoords; auto coordinates = pre40Model.getComponentList(); for (auto& coord : coordinates) { const Coordinate::MotionType oldMotionType = coord.getUserSpecifiedMotionTypePriorTo40(); const Coordinate::MotionType motionType = coord.getMotionType(); - + if ((oldMotionType != Coordinate::MotionType::Undefined) && (oldMotionType != motionType)) { problemCoords.push_back(&coord); } } - + if (problemCoords.size() == 0) return nullptr; - + std::unique_ptr updatedKinematics(kinematics.clone()); // Cycle the inconsistent Coordinates for (const auto& coord : problemCoords) { // Get the corresponding column of data and if in degrees // undo the radians to degrees conversion on that column. int ix = updatedKinematics->getStateIndex(coord->getName()); - + if (ix < 0) { log_warn("updateKinematicsStorageForUpdatedModel(): motion '{}' " "does not contain inconsistent coordinate '{}'.)", @@ -168,12 +169,12 @@ OpenSim::updatePre40KinematicsStorageFor40MotionType(const Model& pre40Model, return updatedKinematics; } - + void OpenSim::updatePre40KinematicsFilesFor40MotionType(const Model& model, const std::vector& filePaths, std::string suffix) { - // Cycle through the data files + // Cycle through the data files for (const auto& filePath : filePaths) { Storage motion(filePath); auto updatedMotion = @@ -207,7 +208,7 @@ void OpenSim::updateSocketConnecteesBySearch(Model& model) socket.finalizeConnection(model); } catch (const ComponentNotFoundOnSpecifiedPath&) { const ComponentPath path(socket.getConnecteePath()); - if (path.getNumPathLevels() >= 1) { + if (path.getNumPathLevels() >= 1) { const Component* found = model.findComponent(path.getComponentName()); if (found) { @@ -446,42 +447,48 @@ TimeSeriesTableVec3 OpenSim::createSyntheticIMUAccelerationSignals( } void OpenSim::appendCoupledCoordinateValues( - OpenSim::TimeSeriesTable& table, const OpenSim::Model& model) { - + OpenSim::TimeSeriesTable& table, const OpenSim::Model& model, + bool overwriteExistingColumns) { + const CoordinateSet& coordinateSet = model.getCoordinateSet(); - const auto& couplerConstraints = + const auto& couplerConstraints = model.getComponentList(); for (const auto& couplerConstraint : couplerConstraints) { - + // Get the dependent coordinate and check if the table already contains - // values for it. If so, skip this constraint. + // values for it. If so, skip this constraint (unless we are + // overwriting existing columns). const Coordinate& coordinate = coordinateSet.get( couplerConstraint.getDependentCoordinateName()); - const std::string& coupledCoordinatePath = + const std::string& coupledCoordinatePath = fmt::format("{}/value", coordinate.getAbsolutePathString()); if (table.hasColumn(coupledCoordinatePath)) { - continue; + if (overwriteExistingColumns) { + table.removeColumn(coupledCoordinatePath); + } else { + continue; + } } - + // Get the paths to the independent coordinate values. - const Array& independentCoordinateNames = + const Array& independentCoordinateNames = couplerConstraint.getIndependentCoordinateNames(); std::vector independentCoordinatePaths; for (int i = 0; i < independentCoordinateNames.getSize(); ++i) { const Coordinate& independentCoordinate = coordinateSet.get( independentCoordinateNames[i]); independentCoordinatePaths.push_back( - fmt::format("{}/value", + fmt::format("{}/value", independentCoordinate.getAbsolutePathString())); OPENSIM_THROW_IF( !table.hasColumn(independentCoordinatePaths.back()), - Exception, + Exception, "Expected the coordinates table to contain a column with " "label '{}', but it does not.", independentCoordinatePaths.back()) } - - // Compute the dependent coordinate values from the function in the + + // Compute the dependent coordinate values from the function in the // CoordinateCouplerConstraint. SimTK::Vector independentValues( (int)independentCoordinatePaths.size(), 0.0); @@ -489,14 +496,14 @@ void OpenSim::appendCoupledCoordinateValues( const Function& function = couplerConstraint.getFunction(); for (int irow = 0; irow < table.getNumRows(); ++irow) { int ival = 0; - for (const auto& independentCoordinatePath : + for (const auto& independentCoordinatePath : independentCoordinatePaths) { - independentValues[ival++] = + independentValues[ival++] = table.getDependentColumn(independentCoordinatePath)[irow]; } newColumn[irow] = function.calcValue(independentValues); } - + // Append the new column to the table. table.appendColumn(coupledCoordinatePath, newColumn); } @@ -504,14 +511,14 @@ void OpenSim::appendCoupledCoordinateValues( void OpenSim::computePathLengthsAndMomentArms( - Model model, + Model model, const TimeSeriesTable& coordinateValues, TimeSeriesTable& pathLengths, TimeSeriesTable& momentArms, std::map>& momentArmMap, - int threads, - double momentArmTolerance) { - + double momentArmTolerance, + int threads) { + // Check inputs. OPENSIM_THROW_IF( threads < 1 || threads > (int)std::thread::hardware_concurrency(), @@ -522,18 +529,19 @@ void OpenSim::computePathLengthsAndMomentArms( OPENSIM_THROW_IF(momentArms.getNumRows() != 0, Exception, "Expected 'momentArms' to be empty."); momentArmMap.clear(); - + // Load model. SimTK::State state = model.initSystem(); - + // Load coordinate values. // TODO check coordinate values + // TODO delete coordinate speeds // TableProcessor tableProcessor = TableProcessor(coordinateValues) | // TabOpUseAbsoluteStateNames() | // TabOpAppendCoupledCoordinateValues(); -// TimeSeriesTable coordinateValuesProcessed = +// TimeSeriesTable coordinateValuesProcessed = // tableProcessor.processAndConvertToRadians(model); - + Array stateVariableNames = model.getStateVariableNames(); for (const auto& label : coordinateValues.getColumnLabels()) { OPENSIM_THROW_IF(stateVariableNames.findIndex(label) == -1, Exception, @@ -542,43 +550,43 @@ void OpenSim::computePathLengthsAndMomentArms( } auto statesTrajectory = StatesTrajectory::createFromStatesTable( model, coordinateValues, true, false, false); - + // Determine the maximum number of path and moment arm evaluations. const auto& paths = model.getComponentList(); int numPaths = (int)std::distance(paths.begin(), paths.end()); int numCoordinates = (int)coordinateValues.getNumColumns(); int numColumns = numPaths + (numPaths * numCoordinates); - + // Define helper function for path length and moment arm computations. auto calcPathLengthsAndMomentArmsSubset = [numColumns, numPaths]( - Model model, StatesTrajectory::IteratorRange subsetStates) + Model model, StatesTrajectory::IteratorRange subsetStates) -> SimTK::Matrix { model.initSystem(); - + // Create a matrix to store the results SimTK::Matrix results( (int)std::distance(subsetStates.begin(), subsetStates.end()), numColumns); - + int row = 0; const auto& forces = model.getComponentList(); for (const auto& state : subsetStates) { model.realizePosition(state); - + int ip = 0; int ima = 0; for (const auto& force : forces) { if (force.hasProperty("path")) { - const AbstractPath& path = + const AbstractPath& path = force.getProperty("path").getValue(); - + // Compute path length. results(row, ip++) = path.getLength(state); - + // Compute moment arms. - for (const auto& coordinate : + for (const auto& coordinate : model.getComponentList()) { - results(row, numPaths + ima++) = + results(row, numPaths + ima++) = path.computeMomentArm(state, coordinate); } } @@ -588,45 +596,45 @@ void OpenSim::computePathLengthsAndMomentArms( return results; }; - - // Divide the path length and moment arm computations across multiple + + // Divide the path length and moment arm computations across multiple // threads. int stride = static_cast( std::floor(coordinateValues.getNumRows() / threads)); std::vector> futures; int offset = 0; for (int ithread = 0; ithread < threads; ++ithread) { - StatesTrajectory::const_iterator begin_iter = + StatesTrajectory::const_iterator begin_iter = statesTrajectory.begin() + offset; StatesTrajectory::const_iterator end_iter = (ithread == threads-1) ? - statesTrajectory.end() : + statesTrajectory.end() : statesTrajectory.begin() + offset + stride; - futures.push_back(std::async(std::launch::async, - calcPathLengthsAndMomentArmsSubset, - model, + futures.push_back(std::async(std::launch::async, + calcPathLengthsAndMomentArmsSubset, + model, makeIteratorRange(begin_iter, end_iter))); offset += stride; } - + // Wait for threads to finish and collect results std::vector outputs(threads); for (int i = 0; i < threads; ++i) { outputs[i] = futures[i].get(); } - + // Assemble results into one TimeSeriesTable std::vector time = coordinateValues.getIndependentColumn(); int itime = 0; for (int i = 0; i < threads; ++i) { for (int j = 0; j < outputs[i].nrow(); ++j) { - pathLengths.appendRow(time[itime], outputs[i].block(j, 0, 1, + pathLengths.appendRow(time[itime], outputs[i].block(j, 0, 1, numPaths).getAsRowVector()); momentArms.appendRow(time[itime], outputs[i].block(j, numPaths, 1, numPaths * numCoordinates).getAsRowVector()); itime++; } } - + int ip = 0; int ima = 0; std::vector pathLengthLabels(numPaths); @@ -634,20 +642,22 @@ void OpenSim::computePathLengthsAndMomentArms( const auto& forces = model.getComponentList(); for (const auto& force : forces) { if (force.hasProperty("path")) { - pathLengthLabels[ip++] = + pathLengthLabels[ip++] = fmt::format("{}_length", force.getAbsolutePathString()); - for (const auto& coordinate : + for (const auto& coordinate : model.getComponentList()) { - momentArmLabels[ima++] = fmt::format("{}_moment_arm_{}", + momentArmLabels[ima++] = fmt::format("{}_moment_arm_{}", force.getAbsolutePathString(), coordinate.getName()); } } } + + std::cout << "momentArmLabels size: " << momentArmLabels.size() << std::endl; pathLengths.setColumnLabels(pathLengthLabels); momentArms.setColumnLabels(momentArmLabels); - + // Remove columns for coupled coordinates. - for (const auto& couplerConstraint : + for (const auto& couplerConstraint : model.getComponentList()) { auto momentArmLabel = fmt::format("_moment_arm_{}", couplerConstraint.getDependentCoordinateName()); @@ -657,7 +667,7 @@ void OpenSim::computePathLengthsAndMomentArms( } } } - + // Remove moment arm columns that contain values below the specified // moment arm tolerance. for (const auto& label : momentArms.getColumnLabels()) { @@ -681,8 +691,9 @@ double OpenSim::fitFunctionBasedPathCoefficients( const TimeSeriesTable& pathLengths, const TimeSeriesTable& momentArms, const std::map>& momentArmMap, + std::string outputPath, const int minOrder, const int maxOrder) { - + // Helper functions. // ----------------- // Factorial function. @@ -693,38 +704,41 @@ double OpenSim::fitFunctionBasedPathCoefficients( } return result; }; - + // Initialize model. // ----------------- model.initSystem(); - + // Coordinate references. // ---------------------- const CoordinateSet& coordinateSet = model.getCoordinateSet(); const int numCoordinates = coordinateSet.getSize(); const int numTimes = (int)coordinateValues.getNumRows(); - + // Build a FunctionBasedPath for each path-based force in the model. // ----------------------------------------------------------------- + // Solving A*x = b, where x is the vector of coefficients for the + // FunctionBasedPath, A is a matrix of polynomial terms, and b is a vector + // of path lengths and moment arms. Set functionBasedPaths; const auto forces = model.getComponentList(); const int numForces = (int)std::distance(forces.begin(), forces.end()); SimTK::Vector bestRootMeanSquareErrors(numForces, SimTK::Infinity); int iforce = 0; for (const auto& force : model.getComponentList()) { - - // Check if the current force is dependent on any coordinates in the + + // Check if the current force is dependent on any coordinates in the // model. If not, skip it. - if (momentArmMap.find(force.getAbsolutePathString()) == + if (momentArmMap.find(force.getAbsolutePathString()) == momentArmMap.end()) { bestRootMeanSquareErrors[iforce] = 0.0; ++iforce; continue; } - + // The current force path and the number of coordinates it depends on. const std::string& forcePath = force.getAbsolutePathString(); - std::vector coordinatesNamesThisForce = + std::vector coordinatesNamesThisForce = momentArmMap.at(forcePath); int numCoordinatesThisForce = (int)coordinatesNamesThisForce.size(); std::vector coordinatePathsThisForce; @@ -732,55 +746,53 @@ double OpenSim::fitFunctionBasedPathCoefficients( coordinatePathsThisForce.push_back( coordinateSet.get(coordinateName).getAbsolutePathString()); } - - // The path lengths for this force. - const SimTK::VectorView pathLengthsThisForce = - pathLengths.getDependentColumn( - fmt::format("{}_length", forcePath)); - + + // Initialize the 'b' vector. This is the same for all polynomial + // orders. + SimTK::Vector b(numTimes * (numCoordinatesThisForce + 1), 0.0); + + // The path lengths for this force. This is the first N elements of the + // 'b' vector. + b(0, numTimes) = pathLengths.getDependentColumn( + fmt::format("{}_length", forcePath)); + // The moment arms and coordinate values for this force. - SimTK::Matrix momentArmsThisForce(numTimes, - numCoordinatesThisForce, 0.0); - SimTK::Matrix coordinatesThisForce(numTimes, numCoordinatesThisForce, + SimTK::Matrix coordinatesThisForce(numTimes, numCoordinatesThisForce, 0.0); for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { const std::string& coordinateName = coordinatesNamesThisForce[ic]; - const SimTK::VectorView momentArmsThisCoordinate = - momentArms.getDependentColumn( - fmt::format("{}_moment_arm_{}", forcePath, - coordinateName)); - const SimTK::VectorView coordinateValuesThisCoordinate = - coordinateValues.getDependentColumn(fmt::format("{}/value", + b((ic+1)*numTimes, numTimes) = momentArms.getDependentColumn( + fmt::format("{}_moment_arm_{}", forcePath, coordinateName)); + + const SimTK::VectorView coordinateValuesThisCoordinate = + coordinateValues.getDependentColumn(fmt::format("{}/value", coordinatePathsThisForce[ic])); for (int itime = 0; itime < numTimes; ++itime) { - momentArmsThisForce.set( - itime, ic, momentArmsThisCoordinate[itime]); coordinatesThisForce.set( itime, ic, coordinateValuesThisCoordinate[itime]); } } - + // Polynomial fitting. // ------------------- double bestRootMeanSquareError = SimTK::Infinity; SimTK::Vector bestCoefficients; int bestOrder = minOrder; for (int order = minOrder; order <= maxOrder; ++order) { - + // Initialize the multivariate polynomial function. int n = numCoordinatesThisForce + order; int k = order; - int numCoefficients = + int numCoefficients = factorial(n) / (factorial(k) * factorial(n - k)); SimTK::Vector dummyCoefficients(numCoefficients, 0.0); MultivariatePolynomialFunction dummyFunction(dummyCoefficients, numCoordinatesThisForce, order); - - // Initialize A and b matrices. - SimTK::Matrix A(numTimes * (numCoordinatesThisForce + 1), + + // Initialize the 'A' matrix. + SimTK::Matrix A(numTimes * (numCoordinatesThisForce + 1), numCoefficients, 0.0); - SimTK::Vector b(numTimes * (numCoordinatesThisForce + 1), 0.0); - + // Fill in the A matrix. This contains the polynomial terms for the // path length and moment arms. for (int itime = 0; itime < numTimes; ++itime) { @@ -788,69 +800,139 @@ double OpenSim::fitFunctionBasedPathCoefficients( coordinatesThisForce.row(itime).getAsVector()); for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { - SimTK::Vector termDerivatives = + SimTK::Vector termDerivatives = dummyFunction.getTermDerivatives({ic}, coordinatesThisForce.row(itime).getAsVector()).negate(); - A((ic+1)*numTimes + itime, 0, 1, numCoefficients) = + A((ic+1)*numTimes + itime, 0, 1, numCoefficients) = termDerivatives; } } - // Fill in the b vector. This contains the path lengths and - // moment arms data. - b(0, numTimes) = pathLengthsThisForce; - for (int ic = 0; ic < numCoordinatesThisForce; ++ic) { - b((ic+1)*numTimes, numTimes) = momentArmsThisForce.col(ic); - } - - // Solve the least-squares problem. A is a rectangular matrix with - // full column rank, so we can use the left pseudo-inverse to solve + // Solve the least-squares problem. A is a rectangular matrix with + // full column rank, so we can use the left pseudo-inverse to solve // for the coefficients. SimTK::Matrix pinv_A = (~A * A).invert() * ~A; SimTK::Vector coefficients = pinv_A * b; - + // Calculate the RMS error. SimTK::Vector b_fit = A * coefficients; SimTK::Vector error = b - b_fit; const double rmsError = std::sqrt((error.normSqr() / error.size())); - + // Save best solution. if (rmsError < bestRootMeanSquareErrors[iforce]) { bestRootMeanSquareErrors[iforce] = rmsError; bestCoefficients = coefficients; bestOrder = order; } - + ++order; } - + // Create a FunctionBasedPath for the current path-based force. MultivariatePolynomialFunction lengthFunction; lengthFunction.setDimension(numCoordinatesThisForce); lengthFunction.setOrder(bestOrder); lengthFunction.setCoefficients(bestCoefficients); - + auto functionBasedPath = std::make_unique(); functionBasedPath->setName(forcePath); functionBasedPath->setCoordinatePaths(coordinatePathsThisForce); functionBasedPath->setLengthFunction(lengthFunction); functionBasedPaths.adoptAndAppend(functionBasedPath.release()); - + ++iforce; } - + // Save the function-based paths to a file. // ---------------------------------------- - functionBasedPaths.print("testPolynomialFitting_Set_FunctionBasedPaths.xml"); - + functionBasedPaths.print(outputPath); + // Return the average RMS error. // ----------------------------- - double averageRootMeanSquareError = bestRootMeanSquareErrors.sum() / + double averageRootMeanSquareError = bestRootMeanSquareErrors.sum() / bestRootMeanSquareErrors.size(); - + return averageRootMeanSquareError; } +void OpenSim::sampleAndAppendValues(TimeSeriesTable& values, + int numSamples, double rangeMultiplier) { + + // Generate coordinate bounds. + SimTK::Matrix lowerBounds = values.getMatrix(); + SimTK::Matrix upperBounds = values.getMatrix(); + SimTK::RowVector offset = SimTK::mean(lowerBounds); + OPENSIM_ASSERT_ALWAYS(offset.size() == values.getNumColumns()); + for (int icol = 0; icol < values.getNumColumns(); ++icol) { + + const SimTK::VectorView col = lowerBounds.col(icol); + const SimTK::Real range = SimTK::max(col) - SimTK::min(col); + + lowerBounds.col(icol).updAsVector() -= offset[icol]; + upperBounds.col(icol).updAsVector() -= offset[icol]; + + lowerBounds.col(icol).updAsVector() -= rangeMultiplier * range; + upperBounds.col(icol).updAsVector() += rangeMultiplier * range; + } + + // Create a vector of time indices to sample from. + std::vector sampleTimeIndices; + sampleTimeIndices.reserve(numSamples); + const double dN = (double)lowerBounds.nrow() / (double)numSamples; + for (int i = 0; i < numSamples; ++i) { + sampleTimeIndices.push_back((int)std::floor(i * dN)); + } + + // Create and seed a random number generator. + SimTK::Random::Uniform random; + random.setSeed(static_cast(std::time(nullptr))); + + // Create a matrix to store the samples. + const int numCoordinates = lowerBounds.ncol(); + SimTK::Matrix samples(2*numSamples, numCoordinates, 0.0); + + // Create vector of indices. + std::vector indices(numSamples); + for (int i = 0; i < numSamples; ++i) { + indices[i] = i; + } + + // Generate a sample matrix by randomly permuting the indices. + for (int i = 0; i < numCoordinates; ++i) { + std::random_shuffle(indices.begin(), indices.end()); + for (int j = 0; j < numSamples; ++j) { + samples(j, i) = (random.getValue() + indices[j]) / numSamples; + samples(j, i) *= lowerBounds(sampleTimeIndices[j], i); + samples(j, i) += offset[i]; + } + } + + for (int i = 0; i < numCoordinates; ++i) { + std::random_shuffle(indices.begin(), indices.end()); + for (int j = 0; j < numSamples; ++j) { + samples(j+numSamples, i) = + (random.getValue() + indices[j]) / numSamples; + samples(j+numSamples, i) *= upperBounds(sampleTimeIndices[j], i); + samples(j+numSamples, i) += offset[i]; + } + } + + + // Create new time column equal to twice the number of samples. + const auto& time = values.getIndependentColumn(); + const double dt = time[1] - time[0]; + double timeSampled = time.back(); + for (int itime = 0; itime < 2*numSamples; ++itime) { + timeSampled += dt; + values.appendRow(timeSampled, samples.row(itime)); + } +} + + + + + diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 5e23c156b8..962f9a93c3 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -64,14 +64,14 @@ OSIMSIMULATION_API void updateStateLabels40( const Model& model, std::vector& labels); #ifndef SWIG -/** Not available through scripting. +/** Not available through scripting. @returns nullptr if no update is necessary. @ingroup simulationutil */ OSIMSIMULATION_API std::unique_ptr updatePre40KinematicsStorageFor40MotionType(const Model& pre40Model, const Storage &kinematics); #endif // SWIG - + /** This function can be used to upgrade MOT files generated with versions before 4.0 in which some data columns are associated with coordinates that were incorrectly marked as Rotational (rather than Coupled). Specific @@ -79,20 +79,20 @@ updatePre40KinematicsStorageFor40MotionType(const Model& pre40Model, leg6dof9musc models. In these cases, the patella will visualize incorrectly in the GUI when replaying the kinematics from the MOT file, and Static Optimization will yield incorrect results. - + The new files are written to the same directories as the original files, but with the provided suffix (before the file extension). To overwrite your original files, set the suffix to an emtpty string. - + If the file does not need to be updated, no new file is written. - + Conversion of the data only occurs for files in degrees ("inDegrees=yes" in the header). - + Do not use this function with MOT files generated by 4.0 or later; doing so will cause your data to be altered incorrectly. We do not detect whether or not your MOT file is pre-4.0. - + In OpenSim 4.0, MotionTypes for Coordinates are now determined strictly by the coordinates' owning Joint. In older models, the MotionType, particularly for CustomJoints, were user- @@ -381,27 +381,29 @@ OSIMSIMULATION_API TimeSeriesTableVec3 createSyntheticIMUAccelerationSignals( /// Compute the values of coordinates defined by `CoordinateCouplerConstraint`s /// in the model and append them to the provided `TimeSeriesTable`. The table /// should contain columns with values for all the independent coordinates that -/// define the coupled coordinate. The column labels must be the full path to +/// define the coupled coordinate. The column labels must be the full path to /// the coordinate values (e.g., `/jointset/ground_pelvis/pelvis_tx/value`). /// @ingroup simulationutil OSIMSIMULATION_API void appendCoupledCoordinateValues( - TimeSeriesTable& table, const Model& model); + TimeSeriesTable& table, const Model& model, + bool overwriteExistingColumns = true); -// TODO: 1) will unlock locked coordinates (so must provide data for these -// coordinates) -// 2) coordinate values must have number of columns equal to -// number of independent coordinates (Appends coupled coordinate +// TODO: 0) should any of the TODOs below be handled in this function? +// (probably not, just throw exceptions) +// 1) how to handle locked coordinates? +// 2) coordinate values must have number of columns equal to +// number of independent coordinates (Appends coupled coordinate // values, if missing) // 3) Updates the coordinates table to use absolute state names OSIMSIMULATION_API void computePathLengthsAndMomentArms( - Model model, + Model model, const TimeSeriesTable& coordinateValues, TimeSeriesTable& pathLengths, TimeSeriesTable& momentArms, std::map>& momentArmMap, - int threads = (int)std::thread::hardware_concurrency() - 2, - double momentArmTolerance = 1e-3); + double momentArmTolerance = 1e-3, + int threads = (int)std::thread::hardware_concurrency() - 2); // TODO: 1) expects length and moment arm column names in specific format // 2) returns average RMS error @@ -411,8 +413,25 @@ OSIMSIMULATION_API double fitFunctionBasedPathCoefficients( const TimeSeriesTable& pathLengths, const TimeSeriesTable& momentArms, const std::map>& momentArmMap, - const int minOrder = 2, const int maxOrder = 9); + std::string outputPath, + const int minOrder = 2, const int maxOrder = 6); + +// TODO: 1) move to TableUtilities (better: switch to SimTK::Matrix and move to CommonUilities) +// 2) create independent utilites for Latin hypercube sampling, finding +// 3) k-nearest neighbors, and computing minimax and/or phi-p criteria +OSIMSIMULATION_API void sampleAndAppendValues( + TimeSeriesTable& values, + int numSamples = 1000, + double rangeMultiplier = 2.0); + + + + + + + } // end of namespace OpenSim + #endif // OPENSIM_SIMULATION_UTILITIES_H_ diff --git a/OpenSim/Simulation/TableProcessor.h b/OpenSim/Simulation/TableProcessor.h index 1191bb811b..1059af71ea 100644 --- a/OpenSim/Simulation/TableProcessor.h +++ b/OpenSim/Simulation/TableProcessor.h @@ -229,19 +229,30 @@ class OSIMSIMULATION_API TabOpUseAbsoluteStateNames : public TableOperator { }; /// Invoke SimulationUtilities::appendCoupledCoordinateValues() on the table. -class OSIMSIMULATION_API TabOpAppendCoupledCoordinateValues +class OSIMSIMULATION_API TabOpAppendCoupledCoordinateValues : public TableOperator { - OpenSim_DECLARE_CONCRETE_OBJECT(TabOpAppendCoupledCoordinateValues, + OpenSim_DECLARE_CONCRETE_OBJECT(TabOpAppendCoupledCoordinateValues, TableOperator); public: - TabOpAppendCoupledCoordinateValues() {} + OpenSim_DECLARE_PROPERTY(overwrite_existing_columns, bool, + "Whether to overwrite existing columns for coupled coordinate " + "values in the table (default: true)."); + + TabOpAppendCoupledCoordinateValues() { + constructProperty_overwrite_existing_columns(true); + } + TabOpAppendCoupledCoordinateValues(bool overwriteExistingColumns) + : TabOpAppendCoupledCoordinateValues() { + set_overwrite_existing_columns(overwriteExistingColumns); + } void operate(TimeSeriesTable& table, const Model* model) const override { OPENSIM_THROW_IF(!model, Exception, "Expected a model, but no model was provided."); - appendCoupledCoordinateValues(table, *model); + appendCoupledCoordinateValues(table, *model, + get_overwrite_existing_columns()); } }; From 84f16ae7b3b2c7465ca4e075af9515b13a4ebae9 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 2 Oct 2023 15:50:43 -0700 Subject: [PATCH 10/34] Saving current progress on LHS designs --- OpenSim/Common/CommonUtilities.cpp | 12 +- OpenSim/Common/LatinHypercubeDesign.cpp | 154 +++++++++++++++--------- OpenSim/Common/LatinHypercubeDesign.h | 59 +++++++-- 3 files changed, 154 insertions(+), 71 deletions(-) diff --git a/OpenSim/Common/CommonUtilities.cpp b/OpenSim/Common/CommonUtilities.cpp index 180bf88d14..3168a104b6 100644 --- a/OpenSim/Common/CommonUtilities.cpp +++ b/OpenSim/Common/CommonUtilities.cpp @@ -189,7 +189,8 @@ SimTK::Matrix OpenSim::computeKNearestNeighbors(const SimTK::Matrix& x, OPENSIM_THROW_IF(x.ncol() != y.ncol(), Exception, "Expected the matrices 'x' and 'y' to have the same number of " - "columns, but found {} and {}, respectively.", x.ncol(), y.ncol()); + "columns, but found {} and {}, respectively.", + x.ncol(), y.ncol()); // Initialize the output matrices. SimTK::Matrix distances(y.nrow(), k, 0.0); @@ -199,7 +200,6 @@ SimTK::Matrix OpenSim::computeKNearestNeighbors(const SimTK::Matrix& x, std::vector distancesVec; // Compute distances between the current row in 'y' and all rows in 'x'. - // Skip the distance between the current row in 'y' and itself. for (int ix = 0; ix < x.nrow(); ++ix) { const double distance = (y.row(iy) - x.row(ix)).normSqr(); distancesVec.push_back(distance); @@ -207,9 +207,7 @@ SimTK::Matrix OpenSim::computeKNearestNeighbors(const SimTK::Matrix& x, // Sort the distances in ascending order. std::sort(distancesVec.begin(), distancesVec.end(), - [](const double& a, const double& b) { - return a < b; - }); + [](const double& a, const double& b) { return a < b; }); // Take the first K distances. for (int ik = 0; ik < k; ++ik) { @@ -219,7 +217,3 @@ SimTK::Matrix OpenSim::computeKNearestNeighbors(const SimTK::Matrix& x, return distances; } - -// - - diff --git a/OpenSim/Common/LatinHypercubeDesign.cpp b/OpenSim/Common/LatinHypercubeDesign.cpp index 7ff4de9c14..be1c22ec2c 100644 --- a/OpenSim/Common/LatinHypercubeDesign.cpp +++ b/OpenSim/Common/LatinHypercubeDesign.cpp @@ -22,6 +22,7 @@ * -------------------------------------------------------------------------- */ #include "LatinHypercubeDesign.h" +#include "Exception.h" #include #include @@ -29,79 +30,129 @@ using namespace OpenSim; -double LatinHypercubeDesign::computeMaximinDistance( - const SimTK::Matrix& x) { - SimTK::Matrix nearestNeighbors = computeKNearestNeighbors(x, x, 2); - return SimTK::min(nearestNeighbors.col(1)); +static const std::vector ValidDistanceCriteria = + {"maximin", "phi_p"}; + +void LatinHypercubeDesign::checkConfiguration() const { + OPENSIM_THROW_IF(m_numSamples < 1, Exception, + "The number of samples must be greater than zero."); + OPENSIM_THROW_IF(m_numVariables < 1, Exception, + "The number of variables must be greater than zero."); + + bool distanceCriterionIsValid = + std::find(ValidDistanceCriteria.begin(), + ValidDistanceCriteria.end(), + m_distanceCriterion) == + ValidDistanceCriteria.end(); + OPENSIM_THROW_IF(distanceCriterionIsValid, Exception, + "Invalid distance criterion. You must choose be one of the " + "following: 'maximin', 'phi_p'."); } -double LatinHypercubeDesign::computePhiPDistanceCriterion( - const SimTK::Matrix& x, const SimTK::Matrix& distances, int p) { +SimTK::Matrix LatinHypercubeDesign::generateTranslationalPropagationDesign( + int numSeedPoints) const { + checkConfiguration(); - double sumInverseDistancesP = 0; - for (int i = 0; i < x.nrow(); ++i) { - for (int j = 0; j < x.nrow(); ++j) { - if (i < j) { - sumInverseDistancesP += 1.0 / pow(distances(i, j), p); + if (numSeedPoints != -1) { + // Generate a seed with 'numSeedPoints' based on a seed with a single + // point. + OPENSIM_THROW_IF(numSeedPoints > m_numSamples, Exception, + "The number of seed points must be less than or equal to the " + "number of samples.") + SimTK::Matrix seed = computeTranslationalPropagationDesign(numSeedPoints, + SimTK::Matrix(1, m_numVariables, 1.0)); + return computeTranslationalPropagationDesign(m_numSamples, seed); + + } else { + // If the number of seed points was not specified, then iterate through + // several seed designs and choose the one with the best distance + // criterion. We arbitrarily choose the maximum number of seed points + // to be 10% of the number of samples. + int maxNumSeedPoints = (int)std::ceil(m_numSamples / 10); + SimTK::Matrix bestDesign; + double bestDistance = SimTK::Infinity; + int nSeedPoints = 1; + while (nSeedPoints <= maxNumSeedPoints) { + // Create the seed. + SimTK::Matrix seed = + computeTranslationalPropagationDesign(nSeedPoints, + SimTK::Matrix(1, m_numVariables, 1.0)); + + // Create the Latin hypercube design. + SimTK::Matrix design = computeTranslationalPropagationDesign( + m_numSamples, seed); + + // Compute the distance criterion. + double distance = computeDistanceCriterion(design); + if (distance < bestDistance) { + bestDesign = design; + bestDistance = distance; } + ++nSeedPoints; } + + return bestDesign; } +} + +double LatinHypercubeDesign::computeDistanceCriterion( + const SimTK::Matrix& design) const { + if (m_useMaximinDistanceCriterion) { + return computeMaximinDistanceCriterion(design); + } else { + return computePhiDistanceCriterion(design); + } +} - return pow(sumInverseDistancesP, 1.0 / p); +double LatinHypercubeDesign::computeMaximinDistanceCriterion( + const SimTK::Matrix& design) const { + SimTK::Matrix nearestNeighbors = + computeKNearestNeighbors(design, design, 2); + return -SimTK::min(nearestNeighbors.col(1)); } -SimTK::Matrix LatinHypercubeDesign::computeIntersiteDistanceMatrix( - const SimTK::Matrix& x) { - - // Compute a matrix of distance values, where each element is the distance - // between the corresponding rows in 'x' and 'y'. The elements should be - // d_ij where 1 <= i, j <= n, i != j, where n is the number of rows in - // 'x' and 'y'. - SimTK::Matrix distances(x.nrow(), x.nrow(), 0.0); - for (int i = 0; i < x.nrow(); ++i) { - for (int j = 0; j < x.nrow(); ++j) { - if (i != j) { - distances(i, j) = (x.row(i) - x.row(j)).normSqr(); +double LatinHypercubeDesign::computePhiDistanceCriterion( + const SimTK::Matrix& design) const { + double sumInverseDistancesP = 0; + for (int i = 0; i < design.nrow(); ++i) { + for (int j = 0; j < design.nrow(); ++j) { + if (i < j) { + double distance = (design.row(j) - design.row(i)).norm(); + sumInverseDistancesP += + 1.0 / pow(distance, m_phiDistanceExponent); } } } - return distances; + return pow(sumInverseDistancesP, 1.0 / m_phiDistanceExponent); } SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( - int numSamples, int numVariables, SimTK::Matrix seed, int numSeedPoints) -{ - // Sorting helper functions. - // ------------------------- - // https://stackoverflow.com/questions/1577475/c-sorting-and-keeping-track-of-indexes - auto sort_indexes = [](const std::vector& v) -> std::vector { - std::vector idx(v.size()); - std::iota(idx.begin(), idx.end(), 0); - std::stable_sort(idx.begin(), idx.end(), - [&v](size_t i1, size_t i2) {return v[i1] < v[i2];}); - return idx; - }; + int numSamples, SimTK::Matrix seed) const { - // Same as above, but for SimTK::Vector. - auto sort_indexes_simtk = [](const SimTK::Vector& v) -> std::vector { - std::vector idx(v.size()); + // Sorting helper function. + // ------------------------ + // https://stackoverflow.com/questions/1577475/c-sorting-and-keeping-track-of-indexes + auto sort_indexes = [](const SimTK::Vector& v) -> std::vector { + std::vector idx(v.size()); std::iota(idx.begin(), idx.end(), 0); std::stable_sort(idx.begin(), idx.end(), - [&v](size_t i1, size_t i2) {return v[i1] < v[i2];}); + [&v](int i1, int i2) {return v[i1] < v[i2];}); return idx; }; // Define the size of the TPLHD to be created first. - // ------------------------------------------------ + // ------------------------------------------------- + int numSeedPoints = (int)seed.nrow(); + int numVariables = (int)seed.ncol(); double numDivisions = - std::pow(numSamples / numSeedPoints, 1.0 / numVariables); + std::pow(getNumSamples() / numSeedPoints, 1.0 / getNumVariables()); int numDivisionsRounded = (int)std::ceil(numDivisions); int numSeeds; if (numDivisionsRounded > numDivisions) { numSeeds = (int)std::pow(numDivisionsRounded, numVariables); } else { - OPENSIM_ASSERT_ALWAYS(numSamples % numSeedPoints == 0); + OPENSIM_ASSERT_ALWAYS(getNumSamples() % numSeedPoints == 0); numSeeds = numSamples / numSeedPoints; } int numSamplesRounded = numSeeds * numSeedPoints; @@ -141,7 +192,6 @@ SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( for (int i = 0; i < numVariables; ++i) { // Update the seed with the most recently added points. seed = tplhd.block(0, 0, numSeedPoints, numVariables); - std::cout << "seed: " << seed << std::endl; // Prepare the seed update. for (int j = 0; j < i; ++j) { @@ -153,7 +203,6 @@ SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( } // Fill in each of the divisions. - int numNewSeedPoints = 0; for (int j = 1; j < numDivisionsRounded; ++j) { // Update the seed. @@ -168,8 +217,6 @@ SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( } } - std::cout << "tplhd: " << tplhd << std::endl; - // If necessary, resize the TPLHD. // ------------------------------- if (numSamplesRounded > numSamples) { @@ -179,10 +226,9 @@ SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( SimTK::RowVector center(numVariables, 0.5*numSamplesRounded); // Compute the distance between each point and the center. - std::vector distance; - distance.reserve(numSamplesRounded); + SimTK::Vector distance(numSamplesRounded, 0.0); for (int i = 0; i < numSamplesRounded; ++i) { - distance.push_back((tplhd.row(i) - center).norm()); + distance.set(i, (tplhd.row(i) - center).norm()); } // Resize the TPLHD by taking the 'numSamples' points closest to the @@ -201,8 +247,8 @@ SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( for (int i = 0; i < numVariables; ++i) { // Place the current design at the origin. - std::vector sortedColumnIndexes = - sort_indexes_simtk(tplhdResized.col(i)); + std::vector sortedColumnIndexes = + sort_indexes(tplhdResized.col(i)); SimTK::Matrix tplhdTemp = tplhdResized; for (int j = 0; j < numSamples; ++j) { tplhdResized.updRow(j) = tplhdTemp.row(sortedColumnIndexes[j]); @@ -232,4 +278,4 @@ SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( } else { return tplhd; } -} \ No newline at end of file +} diff --git a/OpenSim/Common/LatinHypercubeDesign.h b/OpenSim/Common/LatinHypercubeDesign.h index 2421bade9e..9a4740f3db 100644 --- a/OpenSim/Common/LatinHypercubeDesign.h +++ b/OpenSim/Common/LatinHypercubeDesign.h @@ -31,21 +31,64 @@ namespace OpenSim { // A class for generating Latin hypercube designs. class OSIMCOMMON_API LatinHypercubeDesign { public: + // CONSTRUCTION LatinHypercubeDesign() = default; + LatinHypercubeDesign(int numSamples, int numVariables) : + m_numSamples(numSamples), m_numVariables(numVariables) {} - SimTK::Matrix computeTranslationalPropagationDesign(int numSamples, - int numVariables, SimTK::Matrix seed, int numSeedPoints); + LatinHypercubeDesign(const LatinHypercubeDesign&) = default; + LatinHypercubeDesign(LatinHypercubeDesign&&) = default; + LatinHypercubeDesign& operator=(const LatinHypercubeDesign&) = default; + LatinHypercubeDesign& operator=(LatinHypercubeDesign&&) = default; + + // GET AND SET + /// num samples + void setNumSamples(int numSamples) { + m_numSamples = numSamples; + } + int getNumSamples() const { + return m_numSamples; + } + + /// num variables + void setNumVariables(int numVariables) { + m_numVariables = numVariables; + } + int getNumVariables() const { + return m_numVariables; + } + + /// distance criterion + void setDistanceCriterion(std::string distanceCriterion) { + m_distanceCriterion = std::move(distanceCriterion); + m_useMaximinDistanceCriterion = m_distanceCriterion == "maximin"; + } + const std::string& getDistanceCriterion() const { + return m_distanceCriterion; + } + + /// tplhs design + SimTK::Matrix generateTranslationalPropagationDesign( + int numSeedPoints = -1) const; + + + + SimTK::Matrix computeTranslationalPropagationDesign( + int numSamples, SimTK::Matrix seed) const; private: - std::string m_criterion = "maximin"; // maximin, phi_p - double computeMaximinDistance(const SimTK::Matrix& x); + double computeDistanceCriterion(const SimTK::Matrix& design) const; + double computeMaximinDistanceCriterion(const SimTK::Matrix& design) const; + double computePhiDistanceCriterion(const SimTK::Matrix& design) const; - SimTK::Matrix computeIntersiteDistanceMatrix( - const SimTK::Matrix& x); + void checkConfiguration() const; - double computePhiPDistanceCriterion(const SimTK::Matrix& x, - const SimTK::Matrix& distances, int p = 50); + int m_numSamples = -1; + int m_numVariables = -1; + std::string m_distanceCriterion = "maximin"; + bool m_useMaximinDistanceCriterion = true; + int m_phiDistanceExponent = 25; }; From 8a5ed384131ebc86ff021c4fae9ca92b690751cc Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 9 Oct 2023 10:34:25 -0700 Subject: [PATCH 11/34] Remove outdated Latin hypercube code --- OpenSim/Common/CommonUtilities.cpp | 34 --- OpenSim/Common/CommonUtilities.h | 10 - OpenSim/Common/LatinHypercubeDesign.cpp | 281 --------------------- OpenSim/Common/LatinHypercubeDesign.h | 97 ------- OpenSim/Simulation/SimulationUtilities.cpp | 13 +- 5 files changed, 7 insertions(+), 428 deletions(-) delete mode 100644 OpenSim/Common/LatinHypercubeDesign.cpp delete mode 100644 OpenSim/Common/LatinHypercubeDesign.h diff --git a/OpenSim/Common/CommonUtilities.cpp b/OpenSim/Common/CommonUtilities.cpp index 3168a104b6..4bdc4adf03 100644 --- a/OpenSim/Common/CommonUtilities.cpp +++ b/OpenSim/Common/CommonUtilities.cpp @@ -183,37 +183,3 @@ SimTK::Real OpenSim::solveBisection( } return midpoint; } - -SimTK::Matrix OpenSim::computeKNearestNeighbors(const SimTK::Matrix& x, - const SimTK::Matrix& y, int k) { - - OPENSIM_THROW_IF(x.ncol() != y.ncol(), Exception, - "Expected the matrices 'x' and 'y' to have the same number of " - "columns, but found {} and {}, respectively.", - x.ncol(), y.ncol()); - - // Initialize the output matrices. - SimTK::Matrix distances(y.nrow(), k, 0.0); - - // Loop over each row in 'y'. - for (int iy = 0; iy < y.nrow(); ++iy) { - std::vector distancesVec; - - // Compute distances between the current row in 'y' and all rows in 'x'. - for (int ix = 0; ix < x.nrow(); ++ix) { - const double distance = (y.row(iy) - x.row(ix)).normSqr(); - distancesVec.push_back(distance); - } - - // Sort the distances in ascending order. - std::sort(distancesVec.begin(), distancesVec.end(), - [](const double& a, const double& b) { return a < b; }); - - // Take the first K distances. - for (int ik = 0; ik < k; ++ik) { - distances.set(iy, ik, distancesVec[ik]); - } - } - - return distances; -} diff --git a/OpenSim/Common/CommonUtilities.h b/OpenSim/Common/CommonUtilities.h index 7c465858b9..a304325e87 100644 --- a/OpenSim/Common/CommonUtilities.h +++ b/OpenSim/Common/CommonUtilities.h @@ -166,16 +166,6 @@ template class ThreadsafeJar { std::condition_variable m_inventoryMonitor; }; -// A C++ function for finding the K nearest nearest neighbors of two -// matrices X and Y, where X is MX-by-N and Y is MY-by-N. Rows of X and Y -// correspond to observations and columns correspond to variables. The function -// should return a the MY-by-K matrices D and IDX. D sorts the distances in each -// row in ascending order. Each row in IDX contains the indices of K closest -// neighbors in X corresponding to the K smallest distances in D. -OSIMCOMMON_API SimTK::Matrix computeKNearestNeighbors(const SimTK::Matrix& x, - const SimTK::Matrix& y, int k = 1); - - } // namespace OpenSim #endif // OPENSIM_COMMONUTILITIES_H_ diff --git a/OpenSim/Common/LatinHypercubeDesign.cpp b/OpenSim/Common/LatinHypercubeDesign.cpp deleted file mode 100644 index be1c22ec2c..0000000000 --- a/OpenSim/Common/LatinHypercubeDesign.cpp +++ /dev/null @@ -1,281 +0,0 @@ -/* -------------------------------------------------------------------------- * - * OpenSim: LatinHypercubeDesign.cpp * - * -------------------------------------------------------------------------- * - * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * - * See http://opensim.stanford.edu and the NOTICE file for more information. * - * OpenSim is developed at Stanford University and supported by the US * - * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * - * through the Warrior Web program. * - * * - * Copyright (c) 2005-2023 Stanford University and the Authors * - * Author(s): Nicholas Bianco * - * * - * Licensed under the Apache License, Version 2.0 (the "License"); you may * - * not use this file except in compliance with the License. You may obtain a * - * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * - * * - * Unless required by applicable law or agreed to in writing, software * - * distributed under the License is distributed on an "AS IS" BASIS, * - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * - * See the License for the specific language governing permissions and * - * limitations under the License. * - * -------------------------------------------------------------------------- */ - -#include "LatinHypercubeDesign.h" -#include "Exception.h" -#include - -#include -#include - -using namespace OpenSim; - -static const std::vector ValidDistanceCriteria = - {"maximin", "phi_p"}; - -void LatinHypercubeDesign::checkConfiguration() const { - OPENSIM_THROW_IF(m_numSamples < 1, Exception, - "The number of samples must be greater than zero."); - OPENSIM_THROW_IF(m_numVariables < 1, Exception, - "The number of variables must be greater than zero."); - - bool distanceCriterionIsValid = - std::find(ValidDistanceCriteria.begin(), - ValidDistanceCriteria.end(), - m_distanceCriterion) == - ValidDistanceCriteria.end(); - OPENSIM_THROW_IF(distanceCriterionIsValid, Exception, - "Invalid distance criterion. You must choose be one of the " - "following: 'maximin', 'phi_p'."); -} - -SimTK::Matrix LatinHypercubeDesign::generateTranslationalPropagationDesign( - int numSeedPoints) const { - checkConfiguration(); - - if (numSeedPoints != -1) { - // Generate a seed with 'numSeedPoints' based on a seed with a single - // point. - OPENSIM_THROW_IF(numSeedPoints > m_numSamples, Exception, - "The number of seed points must be less than or equal to the " - "number of samples.") - SimTK::Matrix seed = computeTranslationalPropagationDesign(numSeedPoints, - SimTK::Matrix(1, m_numVariables, 1.0)); - return computeTranslationalPropagationDesign(m_numSamples, seed); - - } else { - // If the number of seed points was not specified, then iterate through - // several seed designs and choose the one with the best distance - // criterion. We arbitrarily choose the maximum number of seed points - // to be 10% of the number of samples. - int maxNumSeedPoints = (int)std::ceil(m_numSamples / 10); - SimTK::Matrix bestDesign; - double bestDistance = SimTK::Infinity; - int nSeedPoints = 1; - while (nSeedPoints <= maxNumSeedPoints) { - // Create the seed. - SimTK::Matrix seed = - computeTranslationalPropagationDesign(nSeedPoints, - SimTK::Matrix(1, m_numVariables, 1.0)); - - // Create the Latin hypercube design. - SimTK::Matrix design = computeTranslationalPropagationDesign( - m_numSamples, seed); - - // Compute the distance criterion. - double distance = computeDistanceCriterion(design); - if (distance < bestDistance) { - bestDesign = design; - bestDistance = distance; - } - ++nSeedPoints; - } - - return bestDesign; - } -} - -double LatinHypercubeDesign::computeDistanceCriterion( - const SimTK::Matrix& design) const { - if (m_useMaximinDistanceCriterion) { - return computeMaximinDistanceCriterion(design); - } else { - return computePhiDistanceCriterion(design); - } -} - -double LatinHypercubeDesign::computeMaximinDistanceCriterion( - const SimTK::Matrix& design) const { - SimTK::Matrix nearestNeighbors = - computeKNearestNeighbors(design, design, 2); - return -SimTK::min(nearestNeighbors.col(1)); -} - -double LatinHypercubeDesign::computePhiDistanceCriterion( - const SimTK::Matrix& design) const { - double sumInverseDistancesP = 0; - for (int i = 0; i < design.nrow(); ++i) { - for (int j = 0; j < design.nrow(); ++j) { - if (i < j) { - double distance = (design.row(j) - design.row(i)).norm(); - sumInverseDistancesP += - 1.0 / pow(distance, m_phiDistanceExponent); - } - } - } - - return pow(sumInverseDistancesP, 1.0 / m_phiDistanceExponent); -} - -SimTK::Matrix LatinHypercubeDesign::computeTranslationalPropagationDesign( - int numSamples, SimTK::Matrix seed) const { - - // Sorting helper function. - // ------------------------ - // https://stackoverflow.com/questions/1577475/c-sorting-and-keeping-track-of-indexes - auto sort_indexes = [](const SimTK::Vector& v) -> std::vector { - std::vector idx(v.size()); - std::iota(idx.begin(), idx.end(), 0); - std::stable_sort(idx.begin(), idx.end(), - [&v](int i1, int i2) {return v[i1] < v[i2];}); - return idx; - }; - - // Define the size of the TPLHD to be created first. - // ------------------------------------------------- - int numSeedPoints = (int)seed.nrow(); - int numVariables = (int)seed.ncol(); - double numDivisions = - std::pow(getNumSamples() / numSeedPoints, 1.0 / getNumVariables()); - int numDivisionsRounded = (int)std::ceil(numDivisions); - int numSeeds; - if (numDivisionsRounded > numDivisions) { - numSeeds = (int)std::pow(numDivisionsRounded, numVariables); - } else { - OPENSIM_ASSERT_ALWAYS(getNumSamples() % numSeedPoints == 0); - numSeeds = numSamples / numSeedPoints; - } - int numSamplesRounded = numSeeds * numSeedPoints; - - // Reshape the seed to create the first design. - // -------------------------------------------- - if (numSeedPoints == 1) { - seed = SimTK::Matrix(1, numVariables, 1.0); - } else { - SimTK::RowVector uf(numVariables, numSeedPoints); - SimTK::RowVector ut(numVariables, - (numSamplesRounded / numDivisionsRounded) - - numDivisionsRounded * (numVariables - 1) + 1); - SimTK::RowVector a = (ut - 1).elementwiseDivide(uf - 1); - SimTK::RowVector b = ut - a.elementwiseMultiply(uf); - for (int i = 0; i < numSeedPoints; ++i) { - seed.updRow(i).elementwiseMultiplyInPlace(a); - seed.updRow(i) += b; - - // Round all the elements of the seed to the nearest integer. - for (int j = 0; j < numVariables; ++j) { - seed[i][j] = std::round(seed[i][j]); - } - } - } - - // Create the TPLHD. - // ----------------- - SimTK::Matrix tplhd(numSamplesRounded, numVariables); - - // Fill in the first seed. - for (int i = 0; i < numSeedPoints; ++i) { - tplhd.updRow(i) = seed.row(i); - } - - SimTK::RowVector seedUpdate(numVariables, 1.0); - for (int i = 0; i < numVariables; ++i) { - // Update the seed with the most recently added points. - seed = tplhd.block(0, 0, numSeedPoints, numVariables); - - // Prepare the seed update. - for (int j = 0; j < i; ++j) { - seedUpdate[j] = std::pow(numDivisionsRounded, i - 1); - } - seedUpdate[i] = numSamplesRounded / numDivisionsRounded; - for (int j = i + 1; j < numVariables; ++j) { - seedUpdate[j] = std::pow(numDivisionsRounded, i); - } - - // Fill in each of the divisions. - for (int j = 1; j < numDivisionsRounded; ++j) { - - // Update the seed. - for (int k = 0; k < (int)seed.nrow(); ++k) { - seed.updRow(k) += seedUpdate; - } - - // Update the TPLHD. - tplhd.updBlock(numSeedPoints, 0, (int)seed.nrow(), numVariables) = - seed; - numSeedPoints += (int)seed.nrow(); - } - } - - // If necessary, resize the TPLHD. - // ------------------------------- - if (numSamplesRounded > numSamples) { - SimTK::Matrix tplhdResized(numSamples, numVariables); - - // Center the design space. - SimTK::RowVector center(numVariables, 0.5*numSamplesRounded); - - // Compute the distance between each point and the center. - SimTK::Vector distance(numSamplesRounded, 0.0); - for (int i = 0; i < numSamplesRounded; ++i) { - distance.set(i, (tplhd.row(i) - center).norm()); - } - - // Resize the TPLHD by taking the 'numSamples' points closest to the - // center. - int isample = 0; - for (auto i : sort_indexes(distance)) { - if (isample >= numSamples) { - break; - } - tplhdResized.updRow(isample) = tplhd.row(i); - ++isample; - } - - // Re-establish the Latin hypercube conditions. - SimTK::RowVector tplhdMinimums = SimTK::min(tplhdResized); - for (int i = 0; i < numVariables; ++i) { - - // Place the current design at the origin. - std::vector sortedColumnIndexes = - sort_indexes(tplhdResized.col(i)); - SimTK::Matrix tplhdTemp = tplhdResized; - for (int j = 0; j < numSamples; ++j) { - tplhdResized.updRow(j) = tplhdTemp.row(sortedColumnIndexes[j]); - } - tplhdResized.updCol(i) -= tplhdMinimums[i]; - tplhdResized.updCol(i) += 1; - - // Eliminate empty coordinates. - bool flag = false; - while (!flag) { - std::vector mask; - mask.reserve(numSamples); - SimTK::Vector maskVec(numSamples, 0.0); - for (int j = 0; j < numSamples; ++j) { - mask.push_back(tplhdResized(j, i) != j+1); - maskVec[j] = mask.back(); - } - - // If all elements in mask are false, then set flag to true. - flag = std::all_of(mask.begin(), mask.end(), - [](bool b) {return !b;}); - - tplhdResized.updCol(i) -= maskVec; - } - } - return tplhdResized; - } else { - return tplhd; - } -} diff --git a/OpenSim/Common/LatinHypercubeDesign.h b/OpenSim/Common/LatinHypercubeDesign.h deleted file mode 100644 index 9a4740f3db..0000000000 --- a/OpenSim/Common/LatinHypercubeDesign.h +++ /dev/null @@ -1,97 +0,0 @@ -#ifndef OPENSIM_LATINHYPERCUBEDESIGN_H -#define OPENSIM_LATINHYPERCUBEDESIGN_H -/* -------------------------------------------------------------------------- * - * OpenSim: LatinHypercubeDesign.h * - * -------------------------------------------------------------------------- * - * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * - * See http://opensim.stanford.edu and the NOTICE file for more information. * - * OpenSim is developed at Stanford University and supported by the US * - * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * - * through the Warrior Web program. * - * * - * Copyright (c) 2005-2023 Stanford University and the Authors * - * Author(s): Nicholas Bianco * - * * - * Licensed under the Apache License, Version 2.0 (the "License"); you may * - * not use this file except in compliance with the License. You may obtain a * - * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * - * * - * Unless required by applicable law or agreed to in writing, software * - * distributed under the License is distributed on an "AS IS" BASIS, * - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * - * See the License for the specific language governing permissions and * - * limitations under the License. * - * -------------------------------------------------------------------------- */ - -#include "osimCommonDLL.h" -#include "CommonUtilities.h" - -namespace OpenSim { - -// A class for generating Latin hypercube designs. -class OSIMCOMMON_API LatinHypercubeDesign { -public: - // CONSTRUCTION - LatinHypercubeDesign() = default; - LatinHypercubeDesign(int numSamples, int numVariables) : - m_numSamples(numSamples), m_numVariables(numVariables) {} - - LatinHypercubeDesign(const LatinHypercubeDesign&) = default; - LatinHypercubeDesign(LatinHypercubeDesign&&) = default; - LatinHypercubeDesign& operator=(const LatinHypercubeDesign&) = default; - LatinHypercubeDesign& operator=(LatinHypercubeDesign&&) = default; - - // GET AND SET - /// num samples - void setNumSamples(int numSamples) { - m_numSamples = numSamples; - } - int getNumSamples() const { - return m_numSamples; - } - - /// num variables - void setNumVariables(int numVariables) { - m_numVariables = numVariables; - } - int getNumVariables() const { - return m_numVariables; - } - - /// distance criterion - void setDistanceCriterion(std::string distanceCriterion) { - m_distanceCriterion = std::move(distanceCriterion); - m_useMaximinDistanceCriterion = m_distanceCriterion == "maximin"; - } - const std::string& getDistanceCriterion() const { - return m_distanceCriterion; - } - - /// tplhs design - SimTK::Matrix generateTranslationalPropagationDesign( - int numSeedPoints = -1) const; - - - - SimTK::Matrix computeTranslationalPropagationDesign( - int numSamples, SimTK::Matrix seed) const; - -private: - - double computeDistanceCriterion(const SimTK::Matrix& design) const; - double computeMaximinDistanceCriterion(const SimTK::Matrix& design) const; - double computePhiDistanceCriterion(const SimTK::Matrix& design) const; - - void checkConfiguration() const; - - int m_numSamples = -1; - int m_numVariables = -1; - std::string m_distanceCriterion = "maximin"; - bool m_useMaximinDistanceCriterion = true; - int m_phiDistanceExponent = 25; - -}; - -} // namespace OpenSim - -#endif // OPENSIM_LATINHYPERCUBEDESIGN_H diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 4c855e7715..88f72320b2 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -694,9 +694,7 @@ double OpenSim::fitFunctionBasedPathCoefficients( std::string outputPath, const int minOrder, const int maxOrder) { - // Helper functions. - // ----------------- - // Factorial function. + // Factorial helper function. auto factorial = [](int n) { int result = 1; for (int i = 1; i <= n; ++i) { @@ -705,6 +703,11 @@ double OpenSim::fitFunctionBasedPathCoefficients( return result; }; + // n-choose-k helper function. + auto nchoosek = [factorial](int n, int k) { + return factorial(n) / (factorial(k) * factorial(n - k)); + }; + // Initialize model. // ----------------- model.initSystem(); @@ -781,10 +784,8 @@ double OpenSim::fitFunctionBasedPathCoefficients( for (int order = minOrder; order <= maxOrder; ++order) { // Initialize the multivariate polynomial function. - int n = numCoordinatesThisForce + order; - int k = order; int numCoefficients = - factorial(n) / (factorial(k) * factorial(n - k)); + nchoosek(numCoordinatesThisForce + order, order); SimTK::Vector dummyCoefficients(numCoefficients, 0.0); MultivariatePolynomialFunction dummyFunction(dummyCoefficients, numCoordinatesThisForce, order); From d16a2ba7c24d67ba6f5653f3cacd96976b05f33d Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 9 Oct 2023 15:28:26 -0700 Subject: [PATCH 12/34] Start fleshing out a FunctionBasedPathFitter class --- OpenSim/Common/Property.cpp | 9 +- OpenSim/Common/Property.h | 374 +++++------ .../Simulation/Model/FunctionBasedPath.cpp | 604 +++++++++++++++++- OpenSim/Simulation/Model/FunctionBasedPath.h | 121 +++- OpenSim/Simulation/SimbodyEngine/Coordinate.h | 88 +-- OpenSim/Simulation/SimulationUtilities.cpp | 436 ------------- OpenSim/Simulation/SimulationUtilities.h | 43 -- 7 files changed, 969 insertions(+), 706 deletions(-) diff --git a/OpenSim/Common/Property.cpp b/OpenSim/Common/Property.cpp index 238b9c46d2..67ef0ee1bd 100644 --- a/OpenSim/Common/Property.cpp +++ b/OpenSim/Common/Property.cpp @@ -52,6 +52,13 @@ isEqual(double a, double b) { return std::abs(a - b) <= 1e-7; } +bool Property::TypeHelper:: + isEqual(const SimTK::Vec2& a, const SimTK::Vec2& b) { + for (int i=0; i < 2; ++i) + if (!Property::TypeHelper::isEqual(a[i],b[i])) + return false; + return true; +} bool Property::TypeHelper:: isEqual(const SimTK::Vec3& a, const SimTK::Vec3& b) { @@ -95,7 +102,7 @@ isEqual(const SimTK::Transform& a, const SimTK::Transform& b) { } //============================================================================= -// PROPERTY +// PROPERTY //============================================================================= // Try a few instantiations so we catch bugs now. diff --git a/OpenSim/Common/Property.h b/OpenSim/Common/Property.h index f0fe6eb7cf..5e99bc0fe5 100644 --- a/OpenSim/Common/Property.h +++ b/OpenSim/Common/Property.h @@ -47,36 +47,36 @@ template class ObjectProperty; // PROPERTY //============================================================================== -/** A %Property\ is a serializable (name, list-of-values) pair, where each +/** A %Property\ is a serializable (name, list-of-values) pair, where each value is of type T. The number of values allowed in the list is an attribute of the property; often it is just a single value. Properties are owned by classes -that derive from %OpenSim's serializable Object base class. The documentation -here is most useful for developers who are interested in creating a new +that derive from %OpenSim's serializable Object base class. The documentation +here is most useful for developers who are interested in creating a new Component, ModelComponent, or other serializable class derived from Object. -A property's contained type T must be a serializable type. Serializable types +A property's contained type T must be a serializable type. Serializable types come in two flavors: - - simple types (like int or string) for which serialization instructions + - simple types (like int or string) for which serialization instructions have been provided, and - object types, in which case type T derives from Object and knows how to serialize itself. -When T is a simple type we'll write T=S and refer to a %Property\ as a +When T is a simple type we'll write T=S and refer to a %Property\ as a "simple property". When T is an object type, we'll write T=O and refer to a %Property\ as an "object property". -In case type O is a still-abstract Object-derived type like Function or -Controller, a %Property\ can hold a mix of any concrete objects derived from +In case type O is a still-abstract Object-derived type like Function or +Controller, a %Property\ can hold a mix of any concrete objects derived from O (e.g., any Object that can be dynamic_cast to a Function can be held by a %Property\). The objects in an object property will themselves have properties so a %Property\ can be viewed as a node in the tree of objects -that constitute an %OpenSim Model. Simple properties %Property\ can be +that constitute an %OpenSim Model. Simple properties %Property\ can be viewed as the terminal nodes of that tree. Properties are thus an integral part -of the structure of an %OpenSim Model; anything contained in a property is +of the structure of an %OpenSim Model; anything contained in a property is owned by that property; deleting the property deletes its contained objects. If -you want to \e reference another Object from within a property, use a string +you want to \e reference another Object from within a property, use a string property to reference it by name; the result is a simple property. It is not permitted for type T to be a pointer or reference. @@ -95,7 +95,7 @@ where \c OTypeName stands for the name of the concrete, Object-derived class being serialized, and \c OContents is the representation generated by that class when asked to serialize itself. -A %Property\ that is restricted to holding exactly one object of +A %Property\ that is restricted to holding exactly one object of type O is called a "one-object property". It could be represented in XML as @code OContents @@ -104,16 +104,16 @@ but we allow a more compact representation for one-object properties: @code OContents @endcode -In the one-object case it is also permissible for the property to be unnamed, -in which case it may be referenced as though its name were the same as the -object type name, and there is no separate "name" attribute. The XML +In the one-object case it is also permissible for the property to be unnamed, +in which case it may be referenced as though its name were the same as the +object type name, and there is no separate "name" attribute. The XML representation for an unnamed property is just: @code OContents @endcode -On input, if a name attribute is seen for an unnamed property it is ignored; -only the object type name tag matters in the unnamed case. Note that only -one-object properties can be unnamed, and no single %OpenSim object can have +On input, if a name attribute is seen for an unnamed property it is ignored; +only the object type name tag matters in the unnamed case. Note that only +one-object properties can be unnamed, and no single %OpenSim object can have more than one unnamed property of the same type.

%Property attributes

@@ -124,16 +124,16 @@ In addition to the name and list of values, every property has the following - The minimum and maximum number of values allowed. - A "used default value" flag. -The "used default value" flag specifies that the value stored with this -property was taken from a default object and not subsequently changed. A -property with this flag set is not written out when a model is serialized. +The "used default value" flag specifies that the value stored with this +property was taken from a default object and not subsequently changed. A +property with this flag set is not written out when a model is serialized.

How to declare properties in your class declaration

-Properties are maintained in a PropertyTable by %OpenSim's Object base class -that is used for all serializable objects. Do not create %Property objects +Properties are maintained in a PropertyTable by %OpenSim's Object base class +that is used for all serializable objects. Do not create %Property objects directly; instead, use the provided macros to declare them in the class -declarations for objects derived from Object. These macros should appear in the +declarations for objects derived from Object. These macros should appear in the header file near the top of your class declaration. Comments that should appear in the generated Doxygen documentation as well as in XML files should be in the comment string; if you have a comment that should appear in Doxygen @@ -141,9 +141,9 @@ documentation but not in XML, then you can place it in a Doxygen comment just above the line where you declare your property. Naming conventions: %OpenSim property names should use lower case letters -with \c words_separated_by_underscores. In contrast, %OpenSim object types -begin with a capital letter and use camel case, that is, -\c MixedUpperAndLowerLikeThis. This prevents any possible collisions between +with \c words_separated_by_underscores. In contrast, %OpenSim object types +begin with a capital letter and use camel case, that is, +\c MixedUpperAndLowerLikeThis. This prevents any possible collisions between property names and object types, allowing both to be used as XML tag identifiers with no conflicts. @@ -157,9 +157,9 @@ names below for more information. // Zero or more values. OpenSim_DECLARE_LIST_PROPERTY(name, T, "property description"); @endcode -In the above, T may be a simple type S or object type O. In the case of a +In the above, T may be a simple type S or object type O. In the case of a single-value property where type T is a type derived from -Object (i.e., T=O), you can declare the property to be unnamed and instead use +Object (i.e., T=O), you can declare the property to be unnamed and instead use the class name of the object type O to identify the property: @code // Exactly one value of object type O required. @@ -172,16 +172,16 @@ Finally, for list properties you can declare restrictions on the allowable list length: @code // List must contain exactly listSize (> 0) elements. - OpenSim_DECLARE_LIST_PROPERTY_SIZE(name, T, listSize, + OpenSim_DECLARE_LIST_PROPERTY_SIZE(name, T, listSize, "property description"); // List must contain at least minSize (> 0) elements. - OpenSim_DECLARE_LIST_PROPERTY_ATLEAST(name, T, minSize, + OpenSim_DECLARE_LIST_PROPERTY_ATLEAST(name, T, minSize, "property description"); // List must contain at most maxSize (> 0) elements. - OpenSim_DECLARE_LIST_PROPERTY_ATMOST(name, T, maxSize, + OpenSim_DECLARE_LIST_PROPERTY_ATMOST(name, T, maxSize, "property description"); // List must contain between minSize (> 0) and maxSize (>minSize) elements. - OpenSim_DECLARE_LIST_PROPERTY_RANGE(name, T, minSize, maxSize, + OpenSim_DECLARE_LIST_PROPERTY_RANGE(name, T, minSize, maxSize, "property description"); @endcode Here is an example of an object declaring two properties: **/ @@ -209,16 +209,16 @@ Here is an example of an object declaring two properties: **/ The constructors for your Object-derived class are required to construct and initialize the properties to whatever default values you want them to have. The above macros will have generated for each property a method for this -purpose. If your property is named prop_name, then the method will be -called constructProperty_prop_name(). (In the case of unnamed -properties, the object type serves as prop_name.) The initial value is -provided as an argument, which is optional for those properties that are -allowed to contain a zero-length value list. Here are the various types of +purpose. If your property is named prop_name, then the method will be +called constructProperty_prop_name(). (In the case of unnamed +properties, the object type serves as prop_name.) The initial value is +provided as an argument, which is optional for those properties that are +allowed to contain a zero-length value list. Here are the various types of generated construction methods: @code // Construct and initialize a single-valued property containing type T. void constructProperty_prop_name(const T& value); - // Construct a property with a zero-length value list. + // Construct a property with a zero-length value list. void constructProperty_prop_name(); // Construct a list property, initializing from a container. template