diff --git a/osgait2d/CMakeLists.txt b/osgait2d/CMakeLists.txt new file mode 100644 index 0000000..4cad5ae --- /dev/null +++ b/osgait2d/CMakeLists.txt @@ -0,0 +1,38 @@ +project(Gait2D) + +cmake_minimum_required(VERSION 2.6) + +LIST(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") +FIND_PACKAGE(OpenSim REQUIRED) + +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 ) + +SET(CMAKE_CXX_FLAGS "-std=c++11") +SET(KIT Gait2D) +SET(UKIT GAIT2D) + +set(GAIT2D_BUILD_DOC FALSE CACHE BOOL + "Create API documentation (requires Doxygen).") + +# Doxygen API documentation +if(GAIT2D_BUILD_DOC) + find_package(Doxygen REQUIRED) + if(DOXYGEN_FOUND) + configure_file(${CMAKE_CURRENT_SOURCE_DIR}/doc/Doxyfile + ${CMAKE_CURRENT_BINARY_DIR}/doc/Doxyfile @ONLY) + add_custom_target(doc + ${DOXYGEN_EXECUTABLE} ${CMAKE_CURRENT_BINARY_DIR}/doc/Doxyfile + WORKING_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/doc + COMMENT "Generating API documentation with Doxygen" VERBATIM) + install(DIRECTORY ${PROJECT_BINARY_DIR}/doc/html/ DESTINATION doc) + endif(DOXYGEN_FOUND) +endif(GAIT2D_BUILD_DOC) + +INCLUDE_DIRECTORIES(${OPENSIMSIMBODY_INCLUDE_DIRS}) + +MESSAGE("${OPENSIMBODY_LIBRARIES}") +ADD_SUBDIRECTORY(src) +ADD_SUBDIRECTORY(tests) diff --git a/osgait2d/README.rst b/osgait2d/README.rst new file mode 100644 index 0000000..55d749f --- /dev/null +++ b/osgait2d/README.rst @@ -0,0 +1,16 @@ +This is an implementation of the gait2d model with the OpenSim API. + + +Gait2D.cpp + +This builds the 9 DoF planar waking model (Gait2D). + +GainScheduledController.cpp + +Data.cpp + +The functions in this file load and deal with the various data. + +Optimize.cpp + +This is the main program for running an optimization. diff --git a/osgait2d/cmake/FindOpenSim.cmake b/osgait2d/cmake/FindOpenSim.cmake new file mode 100644 index 0000000..b2ca791 --- /dev/null +++ b/osgait2d/cmake/FindOpenSim.cmake @@ -0,0 +1,288 @@ +# FindOpenSim.cmake +# ================= +# +# License, etc. +# ------------- +# Simbios National Center for Physics Based Simulation of Biological Structures +# Stanford University +# This cmake file was created in 2013 by Chris Dembia and is in the public +# domain. +# +# +# What is this file? What is it part of? +# -------------------------------------- +# This is a CMake "find" module that will try to find the OpenSim +# musculoskeletal modeling and simulation package installed somewhere on your +# computer. OpenSim is part of the SimTK biosimulation toolkit. For more +# information, see https://simtk.org/home/opensim. +# +# +# How do I incorporate it into my own CMake project? +# ---------------------------------------------------- +# To use this file in a find_packages() command from your own CMakeLists.txt +# file, make sure it is in a directory contained in the CMAKE_MODULE_PATH. You +# can add a directory to that path with a line like this: +# +# list(APPEND CMAKE_MODULE_PATH "path/to/this/file") +# +# It is common for Find*.cmake files to be placed in a 'cmake' folder in the +# root of your project. Then, the line above would become: +# +# list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") +# +# +# How do I benefit from using this file in my own project? +# -------------------------------------------------------- +# To 'run' this file in your CMakeLists.txt, include this line: +# +# find_package(OpenSim REQUIRED) +# +# To use the headers and libraries from OpenSim in your own targets: +# +# include_directories(${OPENSIMSIMBODY_INCLUDE_DIRS}) +# add_executable(myClientApplication ${CLIENT_SOURCE} ${CLIENT_HEADERS}) +# target_link_libraries(myClientApplication ${OPENSIMSIMBODY_LIBRARIES}) +# +# You can omit the 'REQUIRED' above if OpenSim isn't actually required for your +# project. If, in this case, you want to check if OpenSim was found, check the +# value of OPENSIM_FOUND. +# +# +# What variables does this script define? +# --------------------------------------- +# OPENSIM_FOUND - If OpenSim libraries and headers were found. +# OPENSIM_ROOT_DIR - the OpenSim installation directory. +# OPENSIM_INCLUDE_DIRS - location of OpenSim/OpenSim.h. +# OPENSIMSIMBODY_INCLUDE_DIRS - list of two directories: +# location of OpenSim/OpenSim.h +# location of Simbody.h +# OPENSIM_LIB_DIR - location of libosim*.{a,so,dylib}, etc. This directory +# contains Simbody libraries as well. On Windows, this is the same as +# OPENSIM_BIN_DIR. +# OPENSIM_BIN_DIR - location of executables/OpenSim tools. +# OPENSIM_LIBRARIES - suitable for target_link_libraries(). Contains the +# libraries with 'osim' in their name. +# OPENSIMSIMBODY_LIBRARIES - suitable for target_link_libraries(). Contains +# libraries with 'osim' or 'SimTK' in their name. +# +# How does the script work? +# ------------------------- +# To find OpenSim on your computer, we look for the following environment +# variables: +# +# OPENSIM_HOME +# +# If the variables does not exist, we check common locations. If the +# script struggles, or finds the wrong version, you can direct it by setting +# the CMake variable: +# +# OPENSIM_INSTALL_DIR +# +# The value of OPENSIM_INSTALL_DIR has the highest priority. +# +# +# OpenSim depends on Simbody, right? How does that work? +# ------------------------------------------------------ +# Currently (18 Oct 2013), OpenSim distributes a version of Simbody that is +# consistent with the distribution of OpenSim. Most users will use this version +# of Simbody (probably won't have a separate installation of Simbody), and thus +# will use the variables that start with OPENSIMSIMBODY_*. If you'd like to use +# a separate installation of Simbody, use the OPENSIM_* variables above instead. +# Then, you can find_package(Simbody) using Simbody's +# FindSimbody.cmake or SimbodyConfig.cmake scripts. +# +# +# TODO +# ---- +# - *MOST IMPORTANT* In find_path(... PATH_SUFFIXES), detect OpenSim x.x (any +# version number). +# - Deal with the library NameSpace. +# - Version selection. Right now, the version argument to find_package is +# ignored. +# - Static libraries. +# - Should OPENSIM_LIB_DIR point to bin on Windows? +# +# See this website to see how these scripts should be written: +# www.cmake.org/Wiki/CMake:How_To_Find_Libraries +# +# The basic gist of the script below is that we first try to set *_INCLUDE_DIR +# and *_LIBRARY variables. If we DO find OpenSim, then we also set the +# *_INCLUDE_DIRS and *_LIBRARIES variables that the client is expected to use. + +cmake_minimum_required(VERSION 2.8) + +# Search for an OpenSim installation. +# ----------------------------------- +set(OPENSIM_HOME "$ENV{OPENSIM_HOME}") +if(OPENSIM_INSTALL_DIR AND NOT "${OPENSIM_INSTALL_DIR}" STREQUAL "") + set(OPENSIM_SEARCH_PATHS "${OPENSIM_INSTALL_DIR}") +elseif(OPENSIM_HOME) + set(OPENSIM_SEARCH_PATHS "${OPENSIM_HOME}") +else() + # Hunt for the installation. + set(OPENSIM_SEARCH_PATHS) + + # Mac, Linux, Cygwin. + if(UNIX) + list(APPEND OPENSIM_SEARCH_PATHS /usr/local) + # Unlikely (for when OpenSim is distributed through distro package + # managers): + list(APPEND OPENSIM_SEARCH_PATHS /usr) + endif() + + if(APPLE) + list(APPEND OPENSIM_SEARCH_PATHS /Developer) + endif() + + # Windows 32 and 64 bit, Cygwin. + if(WIN32) + if(${CMAKE_SIZEOF_VOID_P} EQUAL 8) + # 64 bit target on 64-bit Windows. + set(PROGFILE_DIR "$ENV{ProgramW6432}") + else() + # Target is 32-bit on 64-bit Windows. + set(PROGFILE_DIR "$ENV{ProgramFiles(x86)}") + if(NOT PROGFILE_DIR) + # On 32-bit Windows. + set(PROGFILE_DIR "$ENV{ProgramFiles}") + endif() + endif() + list(APPEND Simbody_SEARCH_PATHS ${PROGFILE_DIR}) + endif() +endif() + + +# OPENSIM_INCLUDE_DIR and OPENSIMSIMBODY_INCLUDE_DIRS +# --------------------------------------------------- +# We find OpenSim by finding sdk/include/OpenSim/OpenSim.h. +set(OPENSIM_INCLUDE_DIR_DOC + "The location of OpenSim/OpenSim.h and all OpenSim headers.") +set(OPENSIMSIMBODY_INCLUDE_DIR_DOC + "The locations of OpenSim and Simbody headers.") + +find_path(OPENSIM_INCLUDE_DIR + NAMES "OpenSim/OpenSim.h" + PATHS ${OPENSIM_SEARCH_PATHS} + PATH_SUFFIXES "sdk/include" "opensim/sdk/include" "OpenSim/sdk/include" + DOC ${OPENSIM_INCLUDE_DIR_DOC} + ) + +# This change is necessary for Simbody 3.4 and beyond, and is incompatible +# with Simbody 3.3 and below. +if(WIN32) + set(OPENSIM_SIMBODY_INCLUDE_RELPATH "include") +else() + set(OPENSIM_SIMBODY_INCLUDE_RELPATH "simbody") +endif() + +set(OPENSIMSIMBODY_INCLUDE_DIR + ${OPENSIM_INCLUDE_DIR} + ${OPENSIM_INCLUDE_DIR}/SimTK/${OPENSIM_SIMBODY_INCLUDE_RELPATH} + ) + + +# OPENSIM_ROOT_DIR +# ---------------- +# Back out the root installation directory. +get_filename_component(OPENSIM_SDK_DIR "${OPENSIM_INCLUDE_DIR}" PATH) +get_filename_component(OPENSIM_ROOT_DIR "${OPENSIM_SDK_DIR}" PATH) + +# OPENSIM_LIB_DIR and OPENSIM_BIN_DIR +# ----------------------------------- +if(WIN32) + set(OPENSIM_PLATFORM_LIB_RELPATH "sdk/lib") +else() + set(OPENSIM_PLATFORM_LIB_RELPATH "lib") +endif() +set(OPENSIM_LIB_DIR ${OPENSIM_ROOT_DIR}/${OPENSIM_PLATFORM_LIB_RELPATH}) +set(OPENSIM_BIN_DIR ${OPENSIM_ROOT_DIR}/bin) + + +# OPENSIM_LIBRARIES and OPENSIMSIMBODY_LIBRARIES +# ---------------------------------------------- +set(OPENSIM_LIBRARIES_DOC "Suitable for target_link_libraries(). Contains only + the libraries with 'osim' in their name.") +set(OPENSIMSIMBODY_LIBRARIES_DOC "Suitable for target_link_libraries(). + Contains libraries with either 'osim' or 'SimTK' in their name.") + +# This variables are for our purposes only; its name comes from convention: +set(OPENSIM_LIBRARY) + +set(OPENSIM_LIBRARY_LIST + osimCommon osimSimulation osimAnalyses osimActuators osimTools) +set(SIMBODY_LIBRARY_LIST SimTKcommon SimTKmath SimTKsimbody) + + +foreach(LIB_NAME IN LISTS OPENSIM_LIBRARY_LIST) + find_library(FOUND_LIB NAMES ${LIB_NAME} + PATHS "${OPENSIM_LIB_DIR}" + NO_DEFAULT_PATH) + if(FOUND_LIB) + list(APPEND OPENSIM_LIBRARY optimized ${FOUND_LIB}) + endif() + unset(FOUND_LIB CACHE) + + find_library(FOUND_LIB NAMES ${LIB_NAME}_d + PATHS "${OPENSIM_LIB_DIR}" + NO_DEFAULT_PATH) + if(FOUND_LIB) + list(APPEND OPENSIM_LIBRARY debug ${FOUND_LIB}_d) + endif() + unset(FOUND_LIB CACHE) +endforeach() + +# Start off this list of libraries with the OpenSim libraries. +set(OPENSIMSIMBODY_LIBRARY ${OPENSIM_LIBRARY}) + +foreach(LIB_NAME IN LISTS SIMBODY_LIBRARY_LIST) + find_library(FOUND_LIB NAMES ${LIB_NAME} + PATHS "${OPENSIM_LIB_DIR}" + NO_DEFAULT_PATH) + if(FOUND_LIB) + list(APPEND OPENSIMSIMBODY_LIBRARY optimized ${FOUND_LIB}) + endif() + unset(FOUND_LIB CACHE) + + find_library(FOUND_LIB NAMES ${LIB_NAME}_d + PATHS "${OPENSIM_LIB_DIR}" + NO_DEFAULT_PATH) + if(FOUND_LIB) + list(APPEND OPENSIMSIMBODY_LIBRARY debug ${FOUND_LIB}_d) + endif() + unset(FOUND_LIB CACHE) +endforeach() + + +# Wrap up +# ------- +set(OPENSIM_INSTALL_DIR "${OPENSIM_ROOT_DIR}" + CACHE PATH "The OpenSim installation directory." FORCE) + +# This CMake-supplied script provides standard error handling. +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(OpenSim + " + Could NOT find OpenSim. Try setting OPENSIM_INSTALL_DIR, or + create an environment variable OPENSIM_HOME." + OPENSIM_INCLUDE_DIR) + +# OPENSIM_FOUND is set automatically for us by find_package(). +if(OPENSIM_FOUND) + set(OPENSIM_INCLUDE_DIRS ${OPENSIM_INCLUDE_DIR}) + set(OPENSIMSIMBODY_INCLUDE_DIRS ${OPENSIMSIMBODY_INCLUDE_DIR}) + set(OPENSIM_LIBRARIES ${OPENSIM_LIBRARY}) + set(OPENSIMSIMBODY_LIBRARIES ${OPENSIMSIMBODY_LIBRARY}) +endif() + +mark_as_advanced( + OPENSIM_ROOT_DIR + OPENSIM_INCLUDE_DIR + OPENSIMSIMBODY_INCLUDE_DIR + OPENSIM_BIN_DIR + OPENSIM_LIB_DIR + OPENSIM_LIBRARY + OPENSIMSIMBODY_LIBRARY +) + +# The following allows us to change the OpenSim installation we use. +unset(OPENSIM_INCLUDE_DIR CACHE) diff --git a/osgait2d/src/CMakeLists.txt b/osgait2d/src/CMakeLists.txt new file mode 100644 index 0000000..867c9b3 --- /dev/null +++ b/osgait2d/src/CMakeLists.txt @@ -0,0 +1,5 @@ +add_executable(gait2d Gait2D.cpp) +target_link_libraries(gait2d ${OPENSIMSIMBODY_LIBRARIES} yaml-cpp) + +add_executable(control GainScheduledController.cpp) +target_link_libraries(control ${OPENSIMSIMBODY_LIBRARIES}) diff --git a/osgait2d/src/Data.cpp b/osgait2d/src/Data.cpp new file mode 100644 index 0000000..ccc0dd4 --- /dev/null +++ b/osgait2d/src/Data.cpp @@ -0,0 +1,7 @@ +// 1. Load a data file which contains time, joint angles, joint rates, and belt +// speed. The columns should be: t, qb, ..., qg, ub, ..., ug, v (or something +// similar. The file should be a csv file presorted in the correct order so the +// C++ code is minimal. +// 2. Load in a vector of heel strike times for a run. +// 3. Load gain array +// 4. Load tstar array diff --git a/osgait2d/src/GainScheduledController.cpp b/osgait2d/src/GainScheduledController.cpp new file mode 100644 index 0000000..0537053 --- /dev/null +++ b/osgait2d/src/GainScheduledController.cpp @@ -0,0 +1,288 @@ +#include +#include +#include + +#include + +double ComputePercentGait (const double current_time, const SimTK::Vector& heel_strike_times) { + // Given a vector of monotonically increasing heel strike times and the + // current time, return the percent of the gait cycle of the current time. + + int length = heel_strike_times.size(); + + // current_time has to be in between the first and last heel strike times + if (current_time < heel_strike_times(0) || current_time > heel_strike_times(length - 1)) + { + throw std::invalid_argument("The heel strikes times must bound the current time."); + } + + int i; + for (i = 0; i < length; i++){ + if (heel_strike_times(i) > current_time){ + break; + } + } + + double time_at_0 = heel_strike_times(i - 1); + double time_at_100 = heel_strike_times(i); + + return (current_time - time_at_0) / (time_at_100 - time_at_0); +} + +double interpolate(double n1, double n3, double d1, double d2, double d3){ + // n3 - n1 n2 - n1 d2 - d1 + // ------- = ------- => n2 = ------- (n3 - n1) + n1 + // d3 - d1 d2 - d1 d3 - d1 + + return (d2 - d1) / (d3 - d1) * (n3 - n1) + n1; + +} + +double extrapolate(double n1, double n2, double d1, double d2, double d3){ + // n3 - n1 n2 - n1 (d3 - d1) + // ------- = ------- => n3 = --------- (n2 - n1) + n1 + // d3 - d1 d2 - d1 (d2 - d1) + + return (d3 - d1) / (d2 - d1) * (n2 - n1) + n1; +} + +SimTK::Matrix InterpolateGainArray(const double percent_gait_cycle, const std::vector& gain_array){ + // Given a n x q x p gain array (n : num percent gain discretizations, q: num + // controls, p : num sensors) and a percent gait cycle value from 0.0 to 1.0, + // return a gain matrix which is a linear interpolation of the matrix elements + // of the two adjacent matrices. + + + if (percent_gait_cycle < 0.0 || percent_gait_cycle > 1.0) + { + throw std::invalid_argument("The percent gait cycle must be between 0.0 and 1.0."); + } + + // Make a percent gain vector for this gain array based on n. + int n = gain_array.size(); + SimTK::Vector percent_gain_vec(n); + for (int i = 0; i < n; i++){ + percent_gain_vec(i) = i * 1.0 / n; + } + std::cout << "percent_gait_vec = " << percent_gain_vec << std::endl; + + // Find the indice which comes after the provided percent gait cycle. + int i; + for (i = 0; i < n; i++){ + if (percent_gain_vec(i) > percent_gait_cycle){ + break; + } + } + + bool need_to_extrapolate = i == n; + + std::cout << "i = " << i << std::endl; + + double first_percent; + double second_percent; + + SimTK::Matrix first_gain_matrix; + SimTK::Matrix second_gain_matrix; + + if (need_to_extrapolate) + { + first_percent = percent_gain_vec(i - 2); + second_percent = percent_gain_vec(i - 1); + first_gain_matrix = gain_array[i - 2]; + second_gain_matrix = gain_array[i - 1]; + } + else + { + first_percent = percent_gain_vec(i - 1); + second_percent = percent_gain_vec(i); + first_gain_matrix = gain_array[i - 1]; + second_gain_matrix = gain_array[i]; + } + + std::cout << "first_percent = " << first_percent << std::endl; + std::cout << "second_percent = " << second_percent << std::endl; + std::cout << "known_percent = " << percent_gait_cycle << std::endl; + + std::cout << "first_gain_matrix = " << first_gain_matrix << std::endl; + std::cout << "second_gain_matrix = " << second_gain_matrix << std::endl; + + int q = first_gain_matrix.nrow(); + int p = first_gain_matrix.ncol(); + + SimTK::Matrix interpolated_gain_matrix(q, p); + + for (int i = 0; i < q; i++) + { + for (int j = 0; j < p; j++) + { + if (need_to_extrapolate) + { + interpolated_gain_matrix(i, j) = extrapolate(first_gain_matrix(i, j), + second_gain_matrix(i, j), + first_percent, + second_percent, + percent_gait_cycle); + } + else + { + + interpolated_gain_matrix(i, j) = interpolate(first_gain_matrix(i, j), + second_gain_matrix(i, j), + first_percent, + percent_gait_cycle, + second_percent); + } + } + } + + return interpolated_gain_matrix; +} + +class GainScheduledController : public OpenSim::Controller { +OpenSim_DECLARE_CONCRETE_OBJECT(GainScheduledController, OpenSim::Controller); + +public: + // This controller needs three pieces of information on construction: + // gain_array : n vector of q x p matrices where each matrix is gain matrix + // corresponding to a percentage of the gait cycle + // t_star_array : n vector of q x 1 matrices where each matrix is T* vector + // corresponding to a percentage of the gait cycle + // heel strike time array : This is a list of times corresponding to heel + // strike (0% in the gait cycle) + // These should be set on construction and not modified. + GainScheduledController(SimTK::Vector heel_strike_times_input, + std::vector t_star_array_input, + std::vector gain_array_input) + : OpenSim::Controller(), + gain_array(gain_array_input), + t_star_array(t_star_array_input), + heel_strike_times(heel_strike_times_input) + { + } + + void computeControls( const SimTK::State& s, SimTK::Vector& controls) const + { + double t = s.getTime(); + + // Pointers to the joint torque actuators + + const OpenSim::CoordinateActuator* TB = dynamic_cast ( &getActuatorSet().get("TB") ); + const OpenSim::CoordinateActuator* TC = dynamic_cast ( &getActuatorSet().get("TC") ); + const OpenSim::CoordinateActuator* TD = dynamic_cast ( &getActuatorSet().get("TD") ); + const OpenSim::CoordinateActuator* TE = dynamic_cast ( &getActuatorSet().get("TE") ); + const OpenSim::CoordinateActuator* TF = dynamic_cast ( &getActuatorSet().get("TF") ); + const OpenSim::CoordinateActuator* TG = dynamic_cast ( &getActuatorSet().get("TG") ); + + const OpenSim::Coordinate* qb = TB->getCoordinate(); + const OpenSim::Coordinate* qc = TC->getCoordinate(); + const OpenSim::Coordinate* qd = TD->getCoordinate(); + const OpenSim::Coordinate* qe = TE->getCoordinate(); + const OpenSim::Coordinate* qf = TF->getCoordinate(); + const OpenSim::Coordinate* qg = TG->getCoordinate(); + + // Build a sensor vector. + double qb_val = qb->getValue(s); + double qc_val = qc->getValue(s); + double qd_val = qd->getValue(s); + double qe_val = qe->getValue(s); + double qf_val = qf->getValue(s); + double qg_val = qg->getValue(s); + double ub_val = qb->getSpeedValue(s); + double uc_val = qc->getSpeedValue(s); + double ud_val = qd->getSpeedValue(s); + double ue_val = qe->getSpeedValue(s); + double uf_val = qf->getSpeedValue(s); + double ug_val = qg->getSpeedValue(s); + + double s_array[12] = {qb_val, qc_val, qd_val, qe_val, qf_val, qg_val, + ub_val, uc_val, ud_val, ue_val, uf_val, ug_val}; + + SimTK::Matrix x(12, 1, *s_array); + + double percent_gait = ComputePercentGait(t, heel_strike_times); + + // Returns a q x 1 SimTK::Matrix + SimTK::Matrix t_star = InterpolateGainArray(percent_gait, t_star_array); + + // Returns a q x p SimTK::Matrix + SimTK::Matrix k = InterpolateGainArray(percent_gait, gain_array); + + // Compute the control values using the control law. + SimTK::Matrix joint_torques = t_star + k * x; + + // Apply the control values to the controller. + SimTK::Vector TB_vec(1, joint_torques(0, 0)); + SimTK::Vector TC_vec(1, joint_torques(1, 0)); + SimTK::Vector TD_vec(1, joint_torques(2, 0)); + SimTK::Vector TE_vec(1, joint_torques(3, 0)); + SimTK::Vector TF_vec(1, joint_torques(4, 0)); + SimTK::Vector TG_vec(1, joint_torques(5, 0)); + + TB->addInControls(TB_vec, controls); + TC->addInControls(TC_vec, controls); + TD->addInControls(TD_vec, controls); + TE->addInControls(TE_vec, controls); + TF->addInControls(TF_vec, controls); + TG->addInControls(TG_vec, controls); + } + +private: + + SimTK::Vector heel_strike_times; + std::vector t_star_array; + std::vector gain_array; +}; + + +int main() +{ + // Test InterpolateGainArray + + std::vector sample_gain_array(10); + + double matrix_stepper = 0.0; + double row_stepper; + double col_stepper; + + for (SimTK::Matrix& gain_matrix_i : sample_gain_array) { + int num_rows = 2; + int num_cols = 1; + SimTK::Matrix gain_mat(num_rows, num_cols); + matrix_stepper += 1.0; + for (int row_idx=0; row_idx < num_rows; row_idx++){ + row_stepper = row_idx; + for(int col_idx=0; col_idx < num_cols; col_idx++){ + col_stepper = col_idx; + gain_mat(row_idx, col_idx) = matrix_stepper + row_stepper + col_stepper; + } + } + gain_matrix_i = gain_mat; + } + + for (const SimTK::Matrix& gain_matrix_i : sample_gain_array) { + std::cout << gain_matrix_i << std::endl; + } + + SimTK::Matrix answer = InterpolateGainArray(0.87, sample_gain_array); + std::cout << answer << std::endl; + + SimTK::Vec<10, double> percent_gait_cycle; + + for (int i=0; i<10; i++){ + percent_gait_cycle(i) = i * 10.0; + } + + std::cout << percent_gait_cycle << std::endl; + + // Test the ComputePercentGait function. + + SimTK::Vector heel_strike_times(5); + heel_strike_times[0] = 3.25; + heel_strike_times[1] = 4.11; + heel_strike_times[2] = 6.77; + heel_strike_times[3] = 8.9; + heel_strike_times[4] = 9.8; + + std::cout << heel_strike_times << std::endl; + std::cout << ComputePercentGait(5.0, heel_strike_times) << std::endl; +} diff --git a/osgait2d/src/Gait2D.cpp b/osgait2d/src/Gait2D.cpp new file mode 100644 index 0000000..5bb3c7d --- /dev/null +++ b/osgait2d/src/Gait2D.cpp @@ -0,0 +1,908 @@ +#include +#include "yaml-cpp/yaml.h" + +int main() +{ + try { + + YAML::Node parameters = YAML::LoadFile("../../data/example_constants.yml"); + std::cout << parameters["mc"].as() << std::endl; + + OpenSim::Model osimModel = OpenSim::Model(); + + osimModel.setUseVisualizer(true); + + osimModel.setName("Gait2D"); + + osimModel.setGravity(SimTK::Vec3(0, -parameters["g"].as(), 0)); + + OpenSim::Body &ground = osimModel.getGroundBody(); + + /* Floor */ + /* The floor has a longitudinal degree of freedom and can be perturbed + * by a longitudinal force or have a prescribed velocity. I'll treat + * this as an infinite floor sliding under the user (just like a + * treadmill). */ + + double floorMass = 100; + SimTK::Vec3 comLocInBody(0.0, 0.0, 0.0); + SimTK::Inertia bodyInertia(10.0, 10.0, 1.0, 0.0, 0.0, 0.0); + + OpenSim::Body* floor = new OpenSim::Body("Floor", + floorMass, + comLocInBody, + bodyInertia); + + floor->addDisplayGeometry("box.vtp"); + floor->updDisplayer()->setScaleFactors(SimTK::Vec3(1.0, 0.05, 1.0)); + + osimModel.addBody(floor); + + SimTK::Vec3 locationInParent(0.0, 0.0, 0.0); + SimTK::Vec3 orientationInParent(0.0, 0.0, 0.0); + SimTK::Vec3 locationInChild(0.0, 0.0, 0.0); + SimTK::Vec3 orientationInChild(0.0, 0.0, 0.0); + + /* A SliderJoint translates along the x axis */ + OpenSim::SliderJoint *floorToGround = + new OpenSim::SliderJoint("FloorToGround", + ground, + locationInParent, + orientationInParent, + *floor, + locationInChild, + orientationInChild, + false); + + OpenSim::CoordinateSet &floorJoints = floorToGround->upd_CoordinateSet(); + + floorJoints[0].setName("floor_translate_x"); + double translationRangeFloor[2] = {-0.1, 2.0 * 8.0 * 60.0}; // 2 m/s * 8 min * 60 sec/min + floorJoints[0].setRange(translationRangeFloor); + floorJoints[0].setDefaultValue(0.0); + + osimModel.addJoint(floorToGround); + + /* Trunk */ + /* The trunk (hip/torso/head/arms) can translate and rotate relative to + * the fixed ground */ + + double trunkLength = 2.0 * parameters["ya"].as(); + + comLocInBody = SimTK::Vec3(parameters["xa"].as(), + parameters["ya"].as(), + 0.0); + + bodyInertia = SimTK::Inertia(10.0, 10.0, parameters["ia"].as(), + 0.0, 0.0, 0.0); + + OpenSim::Body* trunk = new OpenSim::Body("Trunk", + parameters["ia"].as(), + comLocInBody, + bodyInertia); + + locationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + locationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + + OpenSim::FreeJoint *trunkToFloor = + new OpenSim::FreeJoint("TrunkToGround", + ground, + locationInParent, + orientationInParent, + *trunk, + locationInChild, + orientationInChild, + false); + + /* This trunk translates in the x and y directions and rotates about z. */ + + OpenSim::CoordinateSet &trunkJointCoords = trunkToFloor->upd_CoordinateSet(); + + /* Rotation */ + + trunkJointCoords[0].setName("qa_rotx"); + double xRotRangeTrunk[2] = {-SimTK::Pi, -SimTK::Pi}; + trunkJointCoords[0].setRange(xRotRangeTrunk); + trunkJointCoords[0].setDefaultValue(SimTK::convertDegreesToRadians(0.0)); + trunkJointCoords[0].setDefaultLocked(true); + + trunkJointCoords[1].setName("qa_roty"); + double yRotRangeTrunk[2] = {-SimTK::Pi, SimTK::Pi}; + trunkJointCoords[1].setRange(yRotRangeTrunk); + trunkJointCoords[1].setDefaultValue(SimTK::convertDegreesToRadians(0.0)); + trunkJointCoords[1].setDefaultLocked(true); + + trunkJointCoords[2].setName("qa"); + double zRotRangeTrunk[2] = {-SimTK::Pi, SimTK::Pi}; + trunkJointCoords[2].setRange(zRotRangeTrunk); + trunkJointCoords[2].setDefaultValue(SimTK::convertDegreesToRadians(0.0)); + + /* Translation */ + + trunkJointCoords[3].setName("qax"); + double xTranRangeTrunk[2] = {-10.0, 100.0}; + trunkJointCoords[3].setRange(xTranRangeTrunk); + trunkJointCoords[3].setDefaultValue(0.0); + + trunkJointCoords[4].setName("qay"); + double yTranRangeTrunk[2] = {-1.0, 2.0}; + trunkJointCoords[4].setRange(yTranRangeTrunk); + trunkJointCoords[4].setDefaultValue(1.0); // TODO : This should be some nominal height, probably from measured data. + + trunkJointCoords[5].setName("qaz"); + double zTranRangeTrunk[2] = {-1.0, 1.0}; + trunkJointCoords[5].setRange(zTranRangeTrunk); + trunkJointCoords[5].setDefaultValue(0.0); + trunkJointCoords[5].setDefaultLocked(true); + + /* Visualization Geometry */ + + trunk->addDisplayGeometry("sphere.vtp"); + trunk->updDisplayer()->setScaleFactors(SimTK::Vec3(trunkLength / 10.0, trunkLength, trunkLength / 10.0)); + trunk->updDisplayer()->translate(SimTK::Vec3(0.0, trunkLength / 2.0, 0.0)); + + osimModel.addBody(trunk); + osimModel.addJoint(trunkToFloor); + + /* Right Thigh: Body B */ + + comLocInBody = SimTK::Vec3(parameters["xb"].as(), + parameters["yb"].as(), + 0.0); + + bodyInertia = SimTK::Inertia(10.0, 10.0, parameters["ib"].as(), + 0.0, 0.0, 0.0); + + OpenSim::Body* rightThigh = new OpenSim::Body("RightThigh", + parameters["mb"].as(), + comLocInBody, + bodyInertia); + + double rightThighLength = parameters["lb"].as(); + + locationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + + locationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + + OpenSim::PinJoint *rightThighToTrunk = + new OpenSim::PinJoint("RightThighToTrunk", + *trunk, + locationInParent, + orientationInParent, + *rightThigh, + locationInChild, + orientationInChild, + false); + + OpenSim::CoordinateSet &rightThighJoints = rightThighToTrunk->upd_CoordinateSet(); + + rightThighJoints[0].setName("qb"); + double zRotRangeRightThigh[2] = {-SimTK::convertDegreesToRadians(100.0), + SimTK::convertDegreesToRadians(100.0)}; + rightThighJoints[0].setRange(zRotRangeRightThigh); + rightThighJoints[0].setDefaultValue(SimTK::convertDegreesToRadians(30.0)); + + rightThigh->addDisplayGeometry("sphere.vtp"); + rightThigh->updDisplayer()->setScaleFactors(SimTK::Vec3(rightThighLength / 10.0, + rightThighLength, + rightThighLength / 10.0)); + rightThigh->updDisplayer()->translate(SimTK::Vec3(0.0, -rightThighLength / 2.0, 0.0)); + + osimModel.addBody(rightThigh); + osimModel.addJoint(rightThighToTrunk); + + /* Right Shank : Body C */ + + comLocInBody = SimTK::Vec3(parameters["xc"].as(), + parameters["yc"].as(), + 0.0); + bodyInertia = SimTK::Inertia(10.0, 10.0, parameters["ic"].as(), + 0.0, 0.0, 0.0); + + OpenSim::Body* rightShank = new OpenSim::Body("Right Shank", + parameters["mc"].as(), + comLocInBody, + bodyInertia); + + double rightShankLength = parameters["lc"].as(); + + locationInParent = SimTK::Vec3(0.0, -rightThighLength, 0.0); + orientationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + + locationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + + OpenSim::PinJoint *rightShankToThigh = + new OpenSim::PinJoint("RightShankToThigh", + *rightThigh, + locationInParent, + orientationInParent, + *rightShank, + locationInChild, + orientationInChild, + false); + + OpenSim::CoordinateSet &rightShankJoints = rightShankToThigh->upd_CoordinateSet(); + + rightShankJoints[0].setName("qc"); + double zRotRangeRightShank[2] = {-SimTK::convertDegreesToRadians(100.0), 0.0}; + rightShankJoints[0].setRange(zRotRangeRightShank); + rightShankJoints[0].setDefaultValue(SimTK::convertDegreesToRadians(-30.0)); + + rightShank->addDisplayGeometry("sphere.vtp"); + rightShank->updDisplayer()->setScaleFactors(SimTK::Vec3(rightShankLength / 10.0, + rightShankLength, + rightShankLength / 10.0)); + rightShank->updDisplayer()->translate(SimTK::Vec3(0.0, -rightShankLength / 2.0, 0.0)); + + osimModel.addBody(rightShank); + osimModel.addJoint(rightShankToThigh); + + /* Right Foot : Body D */ + + comLocInBody = SimTK::Vec3(parameters["xd"].as(), + parameters["yd"].as(), + 0.0); + + bodyInertia = SimTK::Inertia(10.0, 10.0, parameters["id"].as(), + 0.0, 0.0, 0.0); + + OpenSim::Body* rightFoot = new OpenSim::Body("Right Foot", + parameters["md"].as(), + comLocInBody, + bodyInertia); + + double rightToeDistance = parameters["txd"].as(); + double rightHeelDistance = parameters["hxd"].as(); + double rightFootDepth = parameters["fyd"].as(); + + double footLength = rightToeDistance - rightHeelDistance; + + locationInParent = SimTK::Vec3(0.0, -rightShankLength, 0.0); + orientationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + + locationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + + OpenSim::PinJoint *rightFootToShank = + new OpenSim::PinJoint("RightFootToShank", + *rightShank, + locationInParent, + orientationInParent, + *rightFoot, + locationInChild, + orientationInChild, + false); + + OpenSim::CoordinateSet &rightFootJoints = rightFootToShank->upd_CoordinateSet(); + + rightFootJoints[0].setName("qd"); + double zRotRangeRightFoot[2] = {-SimTK::convertDegreesToRadians(100.0), 0.0}; + rightFootJoints[0].setRange(zRotRangeRightFoot); + rightFootJoints[0].setDefaultValue(SimTK::convertDegreesToRadians(-30.0)); + + rightFoot->addDisplayGeometry("sphere.vtp"); + rightFoot->updDisplayer()->setScaleFactors(SimTK::Vec3(footLength, + footLength / 10.0, + footLength / 10.0)); + + osimModel.addBody(rightFoot); + osimModel.addJoint(rightFootToShank); + + /* Left Thigh: Body E */ + + comLocInBody = SimTK::Vec3(parameters["xe"].as(), + parameters["ye"].as(), + 0.0); + + bodyInertia = SimTK::Inertia(10.0, 10.0, parameters["ie"].as(), + 0.0, 0.0, 0.0); + + OpenSim::Body* leftThigh = new OpenSim::Body("LeftThigh", + parameters["me"].as(), + comLocInBody, + bodyInertia); + + double leftThighLength = parameters["le"].as(); + + locationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + + locationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + + OpenSim::PinJoint *leftThighToTrunk = + new OpenSim::PinJoint("LeftThighToTrunk", + *trunk, + locationInParent, + orientationInParent, + *leftThigh, + locationInChild, + orientationInChild, + false); + + OpenSim::CoordinateSet &leftThighJoints = leftThighToTrunk->upd_CoordinateSet(); + + leftThighJoints[0].setName("qe"); + double zRotRangeLeftThigh[2] = {-SimTK::convertDegreesToRadians(100.0), + SimTK::convertDegreesToRadians(100.0)}; + leftThighJoints[0].setRange(zRotRangeLeftThigh); + leftThighJoints[0].setDefaultValue(SimTK::convertDegreesToRadians(-10.0)); + + leftThigh->addDisplayGeometry("sphere.vtp"); + leftThigh->updDisplayer()->setScaleFactors(SimTK::Vec3(leftThighLength / 10.0, + leftThighLength, + leftThighLength / 10.0)); + leftThigh->updDisplayer()->translate(SimTK::Vec3(0.0, -leftThighLength / 2.0, 0.0)); + + osimModel.addBody(leftThigh); + osimModel.addJoint(leftThighToTrunk); + + /* Left Shank : Body F */ + + comLocInBody = SimTK::Vec3(parameters["xf"].as(), + parameters["yf"].as(), + 0.0); + bodyInertia = SimTK::Inertia(10.0, 10.0, parameters["if"].as(), + 0.0, 0.0, 0.0); + + OpenSim::Body* leftShank = new OpenSim::Body("LeftShank", + parameters["mf"].as(), + comLocInBody, + bodyInertia); + + double leftShankLength = parameters["lf"].as(); + + locationInParent = SimTK::Vec3(0.0, -leftThighLength, 0.0); + orientationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + + locationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + + OpenSim::PinJoint *leftShankToThigh = + new OpenSim::PinJoint("LeftShankToThigh", + *leftThigh, + locationInParent, + orientationInParent, + *leftShank, + locationInChild, + orientationInChild, + false); + + OpenSim::CoordinateSet &leftShankJoints = leftShankToThigh->upd_CoordinateSet(); + + leftShankJoints[0].setName("qf"); + double zRotRangeLeftShank[2] = {-SimTK::convertDegreesToRadians(100.0), 0.0}; + leftShankJoints[0].setRange(zRotRangeLeftShank); + leftShankJoints[0].setDefaultValue(SimTK::convertDegreesToRadians(-30.0)); + + leftShank->addDisplayGeometry("sphere.vtp"); + leftShank->updDisplayer()->setScaleFactors(SimTK::Vec3(leftShankLength / 10.0, + leftShankLength, + leftShankLength / 10.0)); + leftShank->updDisplayer()->translate(SimTK::Vec3(0.0, -leftShankLength / 2.0, 0.0)); + + osimModel.addBody(leftShank); + osimModel.addJoint(leftShankToThigh); + + /* Left Foot : Body G */ + + comLocInBody = SimTK::Vec3(parameters["xg"].as(), + parameters["yg"].as(), + 0.0); + + bodyInertia = SimTK::Inertia(10.0, 10.0, parameters["ig"].as(), + 0.0, 0.0, 0.0); + + OpenSim::Body* leftFoot = new OpenSim::Body("Left Foot", + parameters["mg"].as(), + comLocInBody, + bodyInertia); + + double leftToeDistance = parameters["txg"].as(); + double leftHeelDistance = parameters["hxg"].as(); + double leftFootDepth = parameters["fyg"].as(); + + double leftFootLength = leftToeDistance - leftHeelDistance; + + locationInParent = SimTK::Vec3(0.0, -leftShankLength, 0.0); + orientationInParent = SimTK::Vec3(0.0, 0.0, 0.0); + + locationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + orientationInChild = SimTK::Vec3(0.0, 0.0, 0.0); + + OpenSim::PinJoint *leftFootToShank = + new OpenSim::PinJoint("LeftFootToShank", + *leftShank, + locationInParent, + orientationInParent, + *leftFoot, + locationInChild, + orientationInChild, + false); + + OpenSim::CoordinateSet &leftFootJoints = leftFootToShank->upd_CoordinateSet(); + + leftFootJoints[0].setName("qg"); + double zRotRangeLeftFoot[2] = {-SimTK::convertDegreesToRadians(100.0), 0.0}; + leftFootJoints[0].setRange(zRotRangeLeftFoot); + leftFootJoints[0].setDefaultValue(SimTK::convertDegreesToRadians(-30.0)); + + leftFoot->addDisplayGeometry("sphere.vtp"); + leftFoot->updDisplayer()->setScaleFactors(SimTK::Vec3(leftFootLength, + leftFootLength / 10.0, + leftFootLength / 10.0)); + + osimModel.addBody(leftFoot); + osimModel.addJoint(leftFootToShank); + + /* Contact Geometry */ + + OpenSim::ContactHalfSpace* floorContact = + new OpenSim::ContactHalfSpace(SimTK::Vec3(0.0, 0.0, 0.0), + SimTK::Vec3(0.0, 0.0, -SimTK::Pi / 2.0), + *floor, + "FloorContact"); + osimModel.addContactGeometry(floorContact); + + double contactSphereRadius = 0.05; + + /* Hip */ + + SimTK::Vec3 rightHipLocationInTrunk(0.0, 0.0, 0.0); + + OpenSim::ContactSphere* rightHipContact = + new OpenSim::ContactSphere(contactSphereRadius, + rightHipLocationInTrunk, + *trunk, + "RightHipContact"); + + osimModel.addContactGeometry(rightHipContact); + + SimTK::Vec3 leftHipLocationInTrunk(0.0, 0.0, 0.0); + + OpenSim::ContactSphere* leftHipContact = + new OpenSim::ContactSphere(contactSphereRadius, + leftHipLocationInTrunk, + *trunk, + "LeftHipContact"); + osimModel.addContactGeometry(leftHipContact); + + /* Knee */ + + SimTK::Vec3 rightKneeLocationInThigh(0.0, 0.0, 0.0); + + OpenSim::ContactSphere* rightKneeContact = + new OpenSim::ContactSphere(contactSphereRadius, + rightKneeLocationInThigh, + *rightShank, + "RightKneeContact"); + + osimModel.addContactGeometry(rightKneeContact); + + SimTK::Vec3 leftKneeLocationInThigh(0.0, 0.0, 0.0); + + OpenSim::ContactSphere* leftKneeContact = + new OpenSim::ContactSphere(contactSphereRadius, + leftKneeLocationInThigh, + *leftShank, + "LeftKneeContact"); + + osimModel.addContactGeometry(leftKneeContact); + + /* Ankle */ + + SimTK::Vec3 rightAnkleLocationInThigh(0.0, 0.0, 0.0); + + OpenSim::ContactSphere* rightAnkleContact = + new OpenSim::ContactSphere(contactSphereRadius, + rightAnkleLocationInThigh, + *rightShank, + "RightAnkleContact"); + + osimModel.addContactGeometry(rightAnkleContact); + + SimTK::Vec3 leftAnkleLocationInThigh(0.0, 0.0, 0.0); + + OpenSim::ContactSphere* leftAnkleContact = + new OpenSim::ContactSphere(contactSphereRadius, + leftAnkleLocationInThigh, + *leftShank, + "LeftAnkleContact"); + + osimModel.addContactGeometry(leftAnkleContact); + + /* Feet */ + /* A sphere is offset from the heel and toe points. This is slightly + * different than the pygait2d model because it uses a point + * definition instead of a sphere. */ + + double heelContactSphereRadius = 0.06; + double toeContactSphereRadius = 0.04; + + SimTK::Vec3 rightHeelLocationInFoot(rightHeelDistance + heelContactSphereRadius, + rightFootDepth + heelContactSphereRadius, + 0.0); + + OpenSim::ContactSphere* rightHeelContact = + new OpenSim::ContactSphere(heelContactSphereRadius, + rightHeelLocationInFoot, + *rightFoot, + "RightHeelContact"); + + osimModel.addContactGeometry(rightHeelContact); + + SimTK::Vec3 rightToeLocationInFoot(rightToeDistance - toeContactSphereRadius, + rightFootDepth + toeContactSphereRadius, + 0.0); + + OpenSim::ContactSphere* rightToeContact = + new OpenSim::ContactSphere(toeContactSphereRadius, + rightToeLocationInFoot, + *rightFoot, + "RightToeContact"); + + osimModel.addContactGeometry(rightToeContact); + + SimTK::Vec3 leftHeelLocationInFoot(leftHeelDistance + heelContactSphereRadius, + leftFootDepth + heelContactSphereRadius, + 0.0); + + OpenSim::ContactSphere* leftHeelContact = + new OpenSim::ContactSphere(heelContactSphereRadius, + leftHeelLocationInFoot, + *leftFoot, + "LeftHeelContact"); + + osimModel.addContactGeometry(leftHeelContact); + + SimTK::Vec3 leftToeLocationInFoot(leftToeDistance - toeContactSphereRadius, + leftFootDepth + toeContactSphereRadius, + 0.0); + + OpenSim::ContactSphere* leftToeContact = + new OpenSim::ContactSphere(toeContactSphereRadius, + leftToeLocationInFoot, + *leftFoot, + "LeftToeContact"); + + osimModel.addContactGeometry(leftToeContact); + + + /* Contact Forces */ + + // TODO : I may not be mapping these correctly as I couldn't find the + // equation used to compute this force anywhere. + + double stiffness = parameters["kc"].as(); + double dissipation = parameters["cc"].as(); + double staticFriction = parameters["mu"].as(); + double dynamicFriction = parameters["mu"].as(); + double viscosity = parameters["vs"].as(); + + /* Hip */ + OpenSim::HuntCrossleyForce::ContactParameters *rightHipContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + rightHipContactParams->addGeometry("RightHipContact"); + rightHipContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* rightHipForce = + new OpenSim::HuntCrossleyForce(rightHipContactParams); + rightHipForce->setName("RightHipForce"); + + osimModel.addForce(rightHipForce); + + OpenSim::HuntCrossleyForce::ContactParameters *leftHipContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + leftHipContactParams->addGeometry("LeftHipContact"); + leftHipContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* leftHipForce = + new OpenSim::HuntCrossleyForce(leftHipContactParams); + leftHipForce->setName("LeftHipForce"); + + osimModel.addForce(leftHipForce); + + /* Knee */ + OpenSim::HuntCrossleyForce::ContactParameters *rightKneeContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + rightKneeContactParams->addGeometry("RightKneeContact"); + rightKneeContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* rightKneeForce = + new OpenSim::HuntCrossleyForce(rightKneeContactParams); + rightKneeForce->setName("RightKneeForce"); + + osimModel.addForce(rightKneeForce); + + OpenSim::HuntCrossleyForce::ContactParameters *leftKneeContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + leftKneeContactParams->addGeometry("LeftKneeContact"); + leftKneeContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* leftKneeForce = + new OpenSim::HuntCrossleyForce(leftKneeContactParams); + leftKneeForce->setName("LeftKneeForce"); + + osimModel.addForce(leftKneeForce); + + /* Ankle */ + + OpenSim::HuntCrossleyForce::ContactParameters *rightAnkleContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + + rightAnkleContactParams->addGeometry("RightAnkleContact"); + rightAnkleContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* rightAnkleForce = + new OpenSim::HuntCrossleyForce(rightAnkleContactParams); + rightAnkleForce->setName("RightAnkleForce"); + + osimModel.addForce(rightAnkleForce); + + OpenSim::HuntCrossleyForce::ContactParameters *leftAnkleContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + + leftAnkleContactParams->addGeometry("LeftAnkleContact"); + leftAnkleContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* leftAnkleForce = + new OpenSim::HuntCrossleyForce(leftAnkleContactParams); + leftAnkleForce->setName("LeftAnkleForce"); + + osimModel.addForce(leftAnkleForce); + + /* Foot */ + + /* right heel */ + + OpenSim::HuntCrossleyForce::ContactParameters *rightHeelContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + rightHeelContactParams->addGeometry("RightHeelContact"); + rightHeelContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* rightHeelForce = + new OpenSim::HuntCrossleyForce(rightHeelContactParams); + rightHeelForce->setName("RightHeelForce"); + + osimModel.addForce(rightHeelForce); + + /* right toe */ + + OpenSim::HuntCrossleyForce::ContactParameters *rightToeContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + rightToeContactParams->addGeometry("RightToeContact"); + rightToeContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* rightToeForce = + new OpenSim::HuntCrossleyForce(rightToeContactParams); + rightToeForce->setName("RightToeForce"); + + osimModel.addForce(rightToeForce); + + /* left heel */ + + OpenSim::HuntCrossleyForce::ContactParameters *leftHeelContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + leftHeelContactParams->addGeometry("LeftHeelContact"); + leftHeelContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* leftHeelForce = + new OpenSim::HuntCrossleyForce(leftHeelContactParams); + leftHeelForce->setName("LeftHeelForce"); + + osimModel.addForce(leftHeelForce); + + /* left toe */ + + OpenSim::HuntCrossleyForce::ContactParameters *leftToeContactParams = + new OpenSim::HuntCrossleyForce::ContactParameters(stiffness, + dissipation, + staticFriction, + dynamicFriction, + viscosity); + leftToeContactParams->addGeometry("LeftToeContact"); + leftToeContactParams->addGeometry("FloorContact"); + + OpenSim::HuntCrossleyForce* leftToeForce = + new OpenSim::HuntCrossleyForce(leftToeContactParams); + leftToeForce->setName("LeftToeForce"); + + osimModel.addForce(leftToeForce); + + /* Coordinate Limits */ + /* The pygait2d and algait2d models do not yet have coordinate limit + * forces. Plus I want the identification to find these, not impose + * them, so they are commented out. */ + + /* + + double limitStiffness = 1e6; + double limitDamping = 1e5; + double limitTransition = 5.0; // degrees + + */ + + /* Hip */ + /* + OpenSim::CoordinateLimitForce* rightHipLimit = + new OpenSim::CoordinateLimitForce("RHip_rz", + zRotRangeRightThigh[0], + limitStiffness, + zRotRangeRightThigh[1], + limitStiffness, + limitDamping, + limitTransition, + false); + osimModel.addForce(rightHipLimit); + + OpenSim::CoordinateLimitForce* leftHipLimit = + new OpenSim::CoordinateLimitForce("LHip_rz", + zRotRangeLeftThigh[0], + limitStiffness, + zRotRangeLeftThigh[1], + limitStiffness, + limitDamping, + limitTransition, + false); + osimModel.addForce(leftHipLimit); + */ + + /* Knee */ + /* + OpenSim::CoordinateLimitForce* rightKneeLimit = + new OpenSim::CoordinateLimitForce("RKnee_rz", + zRotRangeRightShank[0], + limitStiffness, + zRotRangeRightShank[1], + limitStiffness, + limitDamping, + limitTransition, + false); + osimModel.addForce(rightKneeLimit); + + OpenSim::CoordinateLimitForce* leftKneeLimit = + new OpenSim::CoordinateLimitForce("LKnee_rz", + zRotRangeLeftShank[0], + limitStiffness, + zRotRangeLeftShank[1], + limitStiffness, + limitDamping, + limitTransition, + false); + osimModel.addForce(leftKneeLimit); + + */ + + /* Generalized Loads */ + /* This adds joint torques for the ankle, knee, and hip. */ + + double maxTorque = 1000.0; + + OpenSim::CoordinateActuator* rightHipTorque = new OpenSim::CoordinateActuator("qb"); + rightHipTorque->setName("TB"); + rightHipTorque->setMinControl(-maxTorque); + rightHipTorque->setMaxControl(maxTorque); + osimModel.addForce(rightHipTorque); + + OpenSim::CoordinateActuator* rightKneeTorque = new OpenSim::CoordinateActuator("qc"); + rightKneeTorque->setName("TC"); + rightKneeTorque->setMinControl(-maxTorque); + rightKneeTorque->setMaxControl(maxTorque); + osimModel.addForce(rightKneeTorque); + + OpenSim::CoordinateActuator* rightAnkleTorque = new OpenSim::CoordinateActuator("qd"); + rightAnkleTorque->setName("TD"); + rightAnkleTorque->setMinControl(-maxTorque); + rightAnkleTorque->setMaxControl(maxTorque); + osimModel.addForce(rightAnkleTorque); + + OpenSim::CoordinateActuator* leftHipTorque = new OpenSim::CoordinateActuator("qe"); + leftHipTorque->setName("TE"); + leftHipTorque->setMinControl(-maxTorque); + leftHipTorque->setMaxControl(maxTorque); + osimModel.addForce(leftHipTorque); + + OpenSim::CoordinateActuator* leftKneeTorque = new OpenSim::CoordinateActuator("qf"); + leftKneeTorque->setName("TF"); + leftKneeTorque->setMinControl(-maxTorque); + leftKneeTorque->setMaxControl(maxTorque); + osimModel.addForce(leftKneeTorque); + + OpenSim::CoordinateActuator* leftAnkleTorque = new OpenSim::CoordinateActuator("qg"); + leftAnkleTorque->setName("TG"); + leftAnkleTorque->setMinControl(-maxTorque); + leftAnkleTorque->setMaxControl(maxTorque); + osimModel.addForce(leftAnkleTorque); + + /* Prescribed Motion for the ground */ + /* I'm going to move the ground under the walker */ + // SimTK::PrescribedMotion this is a constraint + + /* Serialize Model */ + + osimModel.print("Gait2D.osim"); + + /* Simulate */ + + + // Configure the model. + SimTK::State& state = osimModel.initSystem(); + + // Add display geometry. + osimModel.updMatterSubsystem().setShowDefaultGeometry(true); + SimTK::Visualizer& viz = osimModel.updVisualizer().updSimbodyVisualizer(); + viz.setBackgroundColor(SimTK::Vec3(1, 1, 1)); + + // Simulate. + SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getSystem()); + OpenSim::Manager manager(osimModel, integrator); + manager.setInitialTime(0); + manager.setFinalTime(10.0); + manager.integrate(state); + + } + catch (OpenSim::Exception ex) + { + std::cout << ex.getMessage() << std::endl; + return 1; + } + catch (SimTK::Exception::Base 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; + } + std::cout << "OpenSim example completed successfully" << std::endl; + std::cout << "Press return to continue" << std::endl; + std::cin.get(); + return 0; +} diff --git a/osgait2d/src/Optimize.cpp b/osgait2d/src/Optimize.cpp new file mode 100644 index 0000000..1548464 --- /dev/null +++ b/osgait2d/src/Optimize.cpp @@ -0,0 +1,238 @@ +#include "optimize.h" + +#include +#include + +using namespace Ipopt; + +// constructor +GaitSystemID::GaitSystemID() +{} + +//destructor +GaitSystemID::~GaitSystemID() +{} + +// returns the size of the problem +bool GaitSystemID::get_nlp_info(Index& n, Index& m, Index& nnz_jac_g, + Index& nnz_h_lag, IndexStyleEnum& index_style) +{ + + // q = 6 # controls + // p = 12 # sensors + // There are 9 DoF, 18 states + // Free model parameters: n x 6 x 12 gains, n x 6 T*'s + // Let's start with n = 10 (number of gain discretization points) + // N ~= 36,000 : 100 hz over 6 minutes (about 3/4 of the 8 minute data from + // each trial) + // The number of free parameters will then be: 36,000 x 18 + 10 x 6 x 12 + 10 + // x 6 = 648780 (780 model parameter unknowns) + n = 648780; + + // The constraints are (N - 1) x 18 = 647982 + // The constraints are the equations of motion evaluated for at every time + // node expect the first one. + m = 647982; + + // There should be only two per row for the xi and xi-1, then values for the + // 780 model parameters. + // num rows = num constraints + // num cols = num free parameters + // There are two non zeros per row per state + a nonzero for each free + // parameter in the dynamic equations (i.e. parameter derivs are zero in the + // kinematic equations) + // (2 * 18) * 647982 + 780 * 647982 / 2 + nnz_jac_g = 276040332; + + // We will let IPOPT estimate the Hessian. + // TODO : I'm not sure if this needs to be set or what. + nnz_h_lag = 10; + + // use the C style indexing (0-based) + index_style = TNLP::C_STYLE; + + return true; +} + +// returns the variable bounds +bool GaitSystemID::get_bounds_info(Index n, Number* x_l, Number* x_u, + Index m, Number* g_l, Number* g_u) +{ + // here, the n and m we gave IPOPT in get_nlp_info are passed back to us. + // If desired, we could assert to make sure they are what we think they are. + assert(n == 648780); + assert(m == 647982); + + // Could set bounds for the states not to deviate too far from the data. That + // would mean having a vector of +/- some bound around the measurements + + // the variables have lower bounds of 1 + for (Index i=0; i<4; i++) { + x_l[i] = 1.0; + } + + // the variables have upper bounds of 5 + for (Index i=0; i<4; i++) { + x_u[i] = 5.0; + } + + // The constraints must all equal zero. (or we could just bound them to be + // small. + for (Index i=0; i