diff options
| -rw-r--r-- | .gitignore | 1 | ||||
| -rw-r--r-- | CMakeLists.txt | 78 | ||||
| -rw-r--r-- | LocomotorPrimitives.cpp | 113 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.cpp | 37 | ||||
| -rw-r--r-- | LocomotorPrimitivesController.h | 17 | ||||
| -rw-r--r-- | LocomotorPrimitivesManager.cpp | 9 | ||||
| -rw-r--r-- | LocomotorPrimitivesManager.h | 14 |
7 files changed, 269 insertions, 0 deletions
diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..636cd94 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,78 @@ +########################################### +## For building with the rest of OpenSim ## +########################################### + +cmake_minimum_required(VERSION 2.6) + +# Define project +PROJECT (LocomotorPrimitives) + +INCLUDE_DIRECTORIES(${OpenSim_SOURCE_DIR} ${OpenSim_SOURCE_DIR}/Vendors) + +SET(OPENSIM_INSTALL_DIR $ENV{OPENSIM_HOME} + CACHE PATH "Top-level directory of OpenSim install") + +# Change name of build target +SET(TARGET exampleMain CACHE TYPE STRING) +# Identify the cpp file(s) that were to be built +FILE(GLOB SOURCE_FILES *.h *.cpp) +SET(SOURCE ${SOURCE_FILES}) + +# To add Debug feature add ";Debug" after Release on the line below +SET(CMAKE_CONFIGURATION_TYPES "RelWithDebInfo;Release" + CACHE STRING "Semicolon separated list of supported configuration types, only supports Debug, Release, MinSizeRel, and RelWithDebInfo, anything else will be ignored." FORCE ) + +# Location of headers +SET(SIMTK_HEADERS_DIR ${OPENSIM_INSTALL_DIR}/sdk/include/SimTK/include) +SET(OPENSIM_HEADERS_DIR ${OPENSIM_INSTALL_DIR}/sdk/include) +INCLUDE_DIRECTORIES(${SIMTK_HEADERS_DIR} ${OPENSIM_HEADERS_DIR}) +# Libraries and dlls +SET(OPENSIM_LIBS_DIR ${OPENSIM_INSTALL_DIR}/sdk/lib) +SET(OPENSIM_DLLS_DIR ${OPENSIM_INSTALL_DIR}/bin) +LINK_DIRECTORIES(${OPENSIM_LIBS_DIR} ${OPENSIM_DLLS_DIR}) + +# Namespace +SET(NameSpace "OpenSim_" CACHE STRING "Prefix for simtk lib names, includes trailing '_'. Leave empty to use stock SimTK libraries.") +MARK_AS_ADVANCED(NameSpace) + +ADD_EXECUTABLE(${TARGET} ${SOURCE}) + +TARGET_LINK_LIBRARIES(${TARGET} + debug osimSimulation_d optimized osimSimulation + debug osimActuators_d optimized osimActuators + debug osimCommon_d optimized osimCommon + debug osimAnalyses_d optimized osimAnalyses + debug osimTools_d optimized osimTools + debug ${NameSpace}SimTKcommon_d optimized ${NameSpace}SimTKcommon + debug ${NameSpace}SimTKmath_d optimized ${NameSpace}SimTKmath + debug ${NameSpace}SimTKsimbody_d optimized ${NameSpace}SimTKsimbody + SimTKlapack + ${PLATFORM_LIBS} +) + +IF(WIN32) + SET(PLATFORM_LIBS pthreadVC2) +ELSE (WIN32) + SET(NameSpace "") + IF(APPLE) + SET(PLATFORM_LIBS SimTKAtlas) + ELSE(APPLE) + SET(PLTAFORM_LIBS SimTKAtlas_Lin_generic) + ENDIF(APPLE) +ENDIF (WIN32) + +# This block copies the additional files into the running directory +# For example vtp, obj files. Add to the end for more extentions +FILE(GLOB DATA_FILES *.vtp *.obj) +FOREACH (dataFile ${DATA_FILES}) + ADD_CUSTOM_COMMAND( + TARGET ${TARGET} + COMMAND ${CMAKE_COMMAND} + ARGS -E copy + ${dataFile} + ${OpenSimTugOfWar_BINARY_DIR}) + ENDFOREACH (dataFile) + +MARK_AS_ADVANCED(CMAKE_INSTALL_PREFIX) +MARK_AS_ADVANCED(EXECUTABLE_OUTPUT_PATH) +MARK_AS_ADVANCED(LIBRARY_OUTPUT_PATH) diff --git a/LocomotorPrimitives.cpp b/LocomotorPrimitives.cpp new file mode 100644 index 0000000..7ceecb4 --- /dev/null +++ b/LocomotorPrimitives.cpp @@ -0,0 +1,113 @@ +/** + * Simulation for the Locomotor Primitives (Dominici et. al 2012) in a Human + * Leg model with 14 Muscles. + * + * Author: Tobias Klauser <tobias.klauser@uzh.ch> + */ + +#include <OpenSim/OpenSim.h> + +#include "LocomotorPrimitivesManager.h" +#include "LocomotorPrimitivesController.h" + +#define NO_SIM 0 + +static const std::string MODEL_NAME = "LocomotorPrimitives"; + +// Define the initial and final simulation time +static const double initialTime = 0.0; +static const double finalTime = 5.0; + +static void constructModel(OpenSim::Model &model) +{ + std::cout << "Constructing " << MODEL_NAME << " model" << std::endl; + + // Define controller for the model + LocomotorPrimitivesController *control = new LocomotorPrimitivesController(0.01); + control->setActuators(model.updActuators()); + model.addController(control); + + // Set default activation and fiber length on all muscles of the model + const OpenSim::Set<OpenSim::Actuator> &actSet = model.getActuators(); + int sz = actSet.getSize(); + std::cout << " + Defining initial muscle states for " << sz << " actuators:" << std::endl; + + // Set default activation and fiber length of all muscles + for (int i = 0; i < sz; i++) { + OpenSim::ActivationFiberLengthMuscle *m = dynamic_cast<OpenSim::ActivationFiberLengthMuscle *>(&actSet.get(i)); + std::cout << " " << m->getName() << std::endl; + m->setDefaultActivation(0.01); + m->setDefaultFiberLength(0.1); + } + + // Create the force reporter for obtaining the forces applied to the model + // during a forward simulation + OpenSim::ForceReporter *reporter = new OpenSim::ForceReporter(&model); + model.addAnalysis(reporter); + + std::cout << "Finished constructing model" << std::endl; +} + +void simulateModel(OpenSim::Model &model) +{ + if (NO_SIM) { + std::cout << "Skipping simulation as per NO_SIM=" << NO_SIM << std::endl; + return; + } + + std::cout << "Simulating model " << MODEL_NAME << std::endl; + std::cout << " + Initializing system" << std::endl; + SimTK::State &si = model.initSystem(); + + std::cout << " + Creating integrator" << std::endl; + // Create the integrator and manager for the simulation + SimTK::RungeKuttaMersonIntegrator integrator(model.getMultibodySystem()); + integrator.setAccuracy(1.0e-4); + std::cout << " + Creating simulation manager" << std::endl; + + LocomotorPrimitivesManager manager(model, integrator); + + // Integrate from initial time to final time + manager.setInitialTime(initialTime); + manager.setFinalTime(finalTime); + std::cout << " + Integrating from " << std::fixed << initialTime << " to " << std::fixed << finalTime << std::endl; + manager.integrate(si); + + // Save the model states from forward integration + OpenSim::Storage statesDegrees(manager.getStateStorage()); + statesDegrees.print(MODEL_NAME + "_states.sto"); + + // Save the forces + OpenSim::Analysis &reporter = model.getAnalysisSet()[0]; + ((OpenSim::ForceReporter &)reporter).getForceStorage().print(MODEL_NAME + "_forces.mot"); +} + +int main(void) +{ + clock_t ts_start = clock(); + + try { + // Create an OpenSim model and set its name + OpenSim::Model osimModel("../../leg6dof9musc.osim"); + osimModel.setName(MODEL_NAME); + constructModel(osimModel); + + osimModel.setUseVisualizer(false); + simulateModel(osimModel); + } catch (OpenSim::Exception ex) { + std::cout << ex.getMessage() << std::endl; + return 1; + } catch (std::exception ex) { + std::cout << ex.what() << std::endl; + return 1; + } catch (...) { + std::cout << "UNRECOGNIZED EXCEPTION" << std::endl; + return 1; + } + + std::cout << "main() routine time = " << 1.e3*(clock()-ts_start)/CLOCKS_PER_SEC << "ms" << std::endl; + + std::cout << "Simulation " + MODEL_NAME << " completed successfully." << std::endl; + std::cin.get(); + return 0; +}
\ No newline at end of file diff --git a/LocomotorPrimitivesController.cpp b/LocomotorPrimitivesController.cpp new file mode 100644 index 0000000..4b7bc1b --- /dev/null +++ b/LocomotorPrimitivesController.cpp @@ -0,0 +1,37 @@ +#include <OpenSim/OpenSim.h> + +#include "LocomotorPrimitivesController.h" + +#define VERBOSE 1 + +static const double TIME_DAMP = 0.1; + +void LocomotorPrimitivesController::computeControls(const SimTK::State &s, SimTK::Vector &controls) const +{ + double t = s.getTime(); + static double last_twitch = t; + + //const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("bifemlh_r")); + const OpenSim::Muscle *rectfem = dynamic_cast<const OpenSim::Muscle *>(&getActuatorSet().get("rect_fem_r")); + + double v = rectfem->getLengtheningSpeed(s); + double act = v * _alpha; + + if (act < 0.0) + act = 0.0; + if (act > 1.0) + act = 1.0; + + if (t - last_twitch < TIME_DAMP) + act = 0.0; + + if (act > 0.0) + last_twitch = t; + + if (VERBOSE && act > 0.0) + std::cout << "(" << std::fixed << t << ") " << "v=" << std::fixed << v << ", act=" << std::fixed << act << std::endl; + + SimTK::Vector ctrl_rectfem(1, act); + + rectfem->addInControls(ctrl_rectfem, controls); +}
\ No newline at end of file diff --git a/LocomotorPrimitivesController.h b/LocomotorPrimitivesController.h new file mode 100644 index 0000000..7d0c08a --- /dev/null +++ b/LocomotorPrimitivesController.h @@ -0,0 +1,17 @@ +#ifndef LOCOMOTORPRIMITIVES_CONTROLLER_H_ +#define LOCOMOTORPRIMITIVES_CONTROLLER_H_ + +#include <OpenSim/OpenSim.h> + +class LocomotorPrimitivesController : public OpenSim::Controller { +OpenSim_DECLARE_CONCRETE_OBJECT(LocomotorPrimitivesController, OpenSim::Controller); + +public: + LocomotorPrimitivesController(double alpha) : OpenSim::Controller(), _alpha(alpha) { } + + void computeControls(const SimTK::State &s, SimTK::Vector &controls) const; +private: + double _alpha; +}; + +#endif /* LOCOMOTORPRIMITIVES_CONTROLLER_H_ */ diff --git a/LocomotorPrimitivesManager.cpp b/LocomotorPrimitivesManager.cpp new file mode 100644 index 0000000..71df313 --- /dev/null +++ b/LocomotorPrimitivesManager.cpp @@ -0,0 +1,9 @@ +#include <OpenSim/OpenSim.h> + +#include "LocomotorPrimitivesManager.h" + +bool LocomotorPrimitivesManager::doIntegration(SimTK::State &s, int step, double dtFirst) +{ + // TODO possiblly additional calculations + return OpenSim::Manager::doIntegration(s, step, dtFirst); +}
\ No newline at end of file diff --git a/LocomotorPrimitivesManager.h b/LocomotorPrimitivesManager.h new file mode 100644 index 0000000..2bd6e3d --- /dev/null +++ b/LocomotorPrimitivesManager.h @@ -0,0 +1,14 @@ +#ifndef LOCOMOTORPRIMITIVES_MANAGER_H_ +#define LOCOMOTORPRIMITIVES_MANAGER_H_ + +#include "OpenSim/OpenSim.h" + +class LocomotorPrimitivesManager : public OpenSim::Manager +{ +public: + LocomotorPrimitivesManager(OpenSim::Model &model, SimTK::Integrator &integrator) + : OpenSim::Manager(model, integrator) { } + bool doIntegration(SimTK::State &s, int step, double dtFirst); +}; + +#endif /* LOCOMOTORPRIMITIVES_MANAGER_H_ */
\ No newline at end of file |