diff --git a/Applications/Analyze/test/subject01_walk1_grf.mot b/Applications/Analyze/test/subject01_walk1_grf.mot index 156b087e2d..d7ed49e99d 100644 --- a/Applications/Analyze/test/subject01_walk1_grf.mot +++ b/Applications/Analyze/test/subject01_walk1_grf.mot @@ -4,7 +4,7 @@ datarows 9009 range 0.000000 15.013300 endheader time ground_force_vx ground_force_vy ground_force_vz ground_force_px ground_force_py ground_force_pz l_ground_force_vx l_ground_force_vy l_ground_force_vz l_ground_force_px l_ground_force_py l_ground_force_pz ground_torque_x ground_torque_y ground_torque_z l_ground_torque_x l_ground_torque_y l_ground_torque_z - 0.00000000 101.51197668 745.46611415 -47.44870554 0.37898285 -0.00750000 0.12774652 17.26938127 20.49185173 -7.46930128 0.81009656 -0.00750000 -0.05354309 0.00000000 13.53783445 0.00000000 1.55503970 -0.75741936 6.88030347 + 0.00000000 101.51197668 745.46611415 -47.44870554 0.37898285 -0.00750000 0.12774652 17.26938127 20.49185173 -7.46930128 0.81009656 -0.00750000 -0.05354309 0.00000000 13.53783445 0.00000000 1.55503970 -0.75741936 6.88030347 0.00170000 103.20438764 743.09734128 -46.86966548 0.37809513 -0.00750000 0.12810137 18.91380164 28.77852948 -7.85752687 0.80990349 -0.00750000 -0.05269428 0.00000000 12.55074067 0.00000000 1.31259011 -0.63901732 6.11982231 0.00330000 104.88449762 740.71354924 -46.28801443 0.37719739 -0.00750000 0.12845514 20.50537239 37.07206456 -8.27207954 0.80968278 -0.00750000 -0.05191409 0.00000000 11.57152058 0.00000000 1.08561207 -0.52055993 5.37458208 0.00500000 106.53992033 738.29961538 -45.70129774 0.37628513 -0.00750000 0.12880460 21.99210933 45.37815047 -8.73769237 0.80943495 -0.00750000 -0.05120007 0.00000000 10.60988307 0.00000000 0.87630199 -0.40049053 4.65578420 diff --git a/Applications/CMakeLists.txt b/Applications/CMakeLists.txt index c1fc930df8..2c85543624 100644 --- a/Applications/CMakeLists.txt +++ b/Applications/CMakeLists.txt @@ -8,6 +8,7 @@ add_subdirectory(IK) add_subdirectory(ID) add_subdirectory(CMC) add_subdirectory(RRA) +add_subdirectory(FunctionBasedPathConversion) add_subdirectory(opensense) add_subdirectory(versionUpdate) diff --git a/Applications/FunctionBasedPathConversion/CMakeLists.txt b/Applications/FunctionBasedPathConversion/CMakeLists.txt new file mode 100644 index 0000000000..8fc5eea99b --- /dev/null +++ b/Applications/FunctionBasedPathConversion/CMakeLists.txt @@ -0,0 +1,7 @@ +if(OPENSIM_BUILD_INDIVIDUAL_APPS) + OpenSimAddApplication(NAME FunctionBasedPathConversion) +endif() + +if(BUILD_TESTING) + subdirs(test) +endif(BUILD_TESTING) diff --git a/Applications/FunctionBasedPathConversion/functionbasedpathconversion.cpp b/Applications/FunctionBasedPathConversion/functionbasedpathconversion.cpp new file mode 100644 index 0000000000..bb1721cf99 --- /dev/null +++ b/Applications/FunctionBasedPathConversion/functionbasedpathconversion.cpp @@ -0,0 +1,162 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: fbp-compiler.cpp * + * -------------------------------------------------------------------------- * + * ScapulothoracicJoint is offered as an addition to the OpenSim API * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * * + * OpenSim is developed at Stanford University and is supported by: * + * * + * - The National Institutes of Health (U54 GM072970, R24 HD065690) * + * - DARPA, through the Warrior Web program * + * - The Chan Zuckerberg Initiative (CZI 2020-218896) * + * * + * Copyright (c) 2005-2021 Stanford University, TU Delft, and the Authors * + * Author(s): Joris Verhagen * + * * + * 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 +#include +#include + +#include +#include +#include +#include + +static const char usage[] = "FunctionBasedPathConversion [--help] INPUT_MODEL OUTPUT_NAME"; +static const char help_text[] = +R"( +ARGS + + --help print this help + +DESCRIPTION + + This compiler tries to convert an .osim model input file (INPUT_MODEL), which + may contain `` or ``s, into a functionally + equivalent model output file (${OUTPUT_NAME}.osim) with associated curve + fitting data (${OUTPUT_NAME}_DATA_${i}.txt). The output model is the same, + but with each `` or `` potentially replaced by a + ``. + + The phrases "functionally equivalent" and "potentially replaced" are + important. The implementation tries to parameterize each input (point-based) + path with the coordinates in the input model. The implementation will + permute through the coordinates, trying to figure out which of them + ultimately affect the input path. If the implementation determines the + coordintate-to-path relationship, it will then try and fit the relationship + to a multidimensional curve. If that curve appears to produce the same + same outputs as the input (point-based) path, it will replace that path + with the function-/curve-based equivalent. This replacement is only + "functionally equivalent" if the model is then used in ways that ensure its + coordinates roughly stay in the same value range as was used during curve + fitting. Input paths are only "potentially replaced" because the implementation + may not be able to parameterize all input paths. + + The primary reason to replace point-based (input) paths with function-based + (output) equivalents is runtime performance. Point-based path computations + can be expensive--especially if those paths are "wrapped" around other geometry-- + whereas function-based paths are (effectively) lookups into precomputed curves. + Those curves are usually smooth, so the resulting path outpus (length, speed, + etc.) are usually smoother when integrated over multiple states. This smoothness + can accelerate error-controlled integration schemes (OpenSim's default). + + If performance isn't an issue for you, you should probably keep using point-based + paths. + + IMPLEMENTATION STEPS (high-level): + + - Reads INPUT_MODEL + - Finds `GeometryPath`/`PointBasedPath`s in INPUT_MODEL + - Parameterizes each input path against each of INPUT_MODEL's coordinates + to produce an n-dimensional Bezier curve fit of those paths + - Saves the curve data to ${OUTPUT_NAME}_DATA_${i}.txt, where `i` is an + arbirary ID that links the `FunctionBasedPath` in the output .osim file + to the Bezier fit's data + - Updates the source model to contain `FunctionBasedPath`s + - Writes the updated model to `${OUTPUT_NAME}.osim` + +EXAMPLE USAGE + + FunctionBasedPathModelTool RajagopalModel.osim RajagopalModel_Precomputed +)"; + +int main(int argc, char **argv) { + // skip exe name + --argc; + ++argv; + + // set during parsing + char const* sourceModelPath = nullptr; + char const* outputName = nullptr; + + // parse CLI args + int nUnnamed = 0; + while (argc) { + const char* arg = *argv; + + if (arg[0] != '-') { // handle unnamed args + switch (nUnnamed) { + case 0: + // INPUT_MODEL + sourceModelPath = arg; + break; + case 1: + // OUTPUT_NAME + outputName = arg; + break; + default: + std::cerr << "FunctionBasedPathModelTool: error: too many arguments: should only supply 'MODEL' and 'OUTPUT_NAME'\n\nUSAGE: " + << usage + << std::endl; + return -1; + } + + ++nUnnamed; + --argc; + ++argv; + continue; + } else { // else: handle named args + if (std::strcmp(arg, "--help") == 0) { + std::cout << "usage: " << usage << '\n' << help_text << std::endl; + return 0; + } else { + std::cerr << "FunctionBasedPathModelTool: error: unknown argument '" + << arg + << "': see --help for usage info"; + return -1; + } + } + } + + // ensure inputs are correct + if (nUnnamed != 2) { + std::cerr << "FunctionBasedPathModelTool: error: too few arguments supplied\n\n" + << usage + << std::endl; + return -1; + } + + assert(argc == 0); + assert(sourceModelPath != nullptr); + assert(outputName != nullptr); + + // run the tool + try { + OpenSim::FunctionBasedPathConversionTool tool(sourceModelPath,outputName); + return tool.run() ? 0 : 1; + } catch(const std::exception& ex) { + OpenSim::log_error("Exception in ID: {}", ex.what()); + return -1; + } +} diff --git a/Applications/FunctionBasedPathConversion/test/CMakeLists.txt b/Applications/FunctionBasedPathConversion/test/CMakeLists.txt new file mode 100644 index 0000000000..e6ab6e5fd3 --- /dev/null +++ b/Applications/FunctionBasedPathConversion/test/CMakeLists.txt @@ -0,0 +1,9 @@ + +file(GLOB TEST_PROGS "test*.cpp") +file(GLOB TEST_FILES *.osim *.xml *.sto *.mot *.trc) + +OpenSimAddTests( + TESTPROGRAMS ${TEST_PROGS} + DATAFILES ${TEST_FILES} + LINKLIBS osimTools + ) diff --git a/Applications/FunctionBasedPathConversion/test/arm26.osim b/Applications/FunctionBasedPathConversion/test/arm26.osim new file mode 100644 index 0000000000..6fb32ab12d --- /dev/null +++ b/Applications/FunctionBasedPathConversion/test/arm26.osim @@ -0,0 +1,1647 @@ + + + + + + The OpenSim Development Team (Reinbolt, J; Seth, A; Habib, A; Hamner, S) adapted from a model originally created by Kate Holzbaur (11/22/04) + + License: + Creative Commons (CCBY 3.0). You are free to distribute, remix, tweak, and build upon this work, even commercially, + as long as you credit us for the original creation. + http://creativecommons.org/licenses/by/3.0/ + + Holzbaur, K.R.S., Murray, W.M., Delp, S.L. A Model of the Upper Extremity for Simulating Musculoskeletal Surgery and Analyzing Neuromuscular Control. + Annals of Biomedical Engineering, vol 33, pp 829–840, 2005 + meters + N + + 0 -9.8066 0 + + + + + 0 + 0 0 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + 0 + 0 0 0 + 0 + 0 + 0 + 0 + 0 + 0 + + + + + ground + + 0 0.8 0 + + 0 0 0 + + 0 0 0 + + 0 0 0 + + + + + + + false + + + + + + + + + ground_ribs.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + ground_spine.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + ground_skull.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + ground_jaw.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + ground_r_clavicle.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + ground_r_scapula.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + 1.37532 -0.294612 2.43596 + -0.043905 -0.0039 0.1478 + true + x + + + + + + + + 1 1 1 + + 1.37532 -0.294612 2.43596 -0.043905 -0.0039 0.1478 + + false + + 4 + + 0.003 + 0.03 + + + + + + + 1.864572 + 0 -0.180496 0 + 0.01481 + 0.004551 + 0.013193 + 0 + 0 + 0 + + + + + + + + + r_shoulder_elev + + -0.05889802 0.0023 0.99826136 + + + + 1 0 + + + + + + + + 0 1 0 + + + + 0 + + + + + + + + 0.99826136 -0 0.05889802 + + + + 0 + + + + + + + + + 1 0 0 + + + + 0 + + + + + + + + 0 1 0 + + + + 0 + + + + + + + + 0 0 1 + + + + 0 + + + + + base + -0.017545 -0.007 0.17 + 0 0 0 + 0 0 0 + 0 0 0 + + + + + + rotational + + 0 + + 0 + + -1.57079633 3.14159265 + + false + + false + + + + + + + false + + + + + + + + + arm_r_humerus.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + 3.00162 -0.853466 2.57419 + -0.0078 -0.0041 -0.0014 + true + z + + + + + + + + 1 1 1 + + 3.00162 -0.853466 2.57419 -0.0078 -0.0041 -0.0014 + + false + + 4 + + 0.035 0.02 0.02 + + + -2.00434 -1.00164 0.975465 + 0.0033 0.0073 0.0003 + true + -y + + + + + + + + 1 1 1 + + -2.00434 -1.00164 0.975465 0.0033 0.0073 0.0003 + + false + + 4 + + 0.025 0.02 0.02 + + + -0.14015 -0.00628319 0.154985 + 0.0028 -0.2919 -0.0069 + true + all + + + + + + + + 1 1 1 + + -0.14015 -0.00628319 0.154985 0.0028 -0.2919 -0.0069 + + false + + 4 + + 0.016 + 0.05 + + + + + + + 1.534315 + 0 -0.181479 0 + 0.019281 + 0.001571 + 0.020062 + 0 + 0 + 0 + + + + + + + + + r_elbow_flex + + 0.04940001 0.03660001 0.99810825 + + + + 1 0 + + + + + + + + 0 1 0 + + + + 0 + + + + + + + + 0.99810825 0 -0.04940001 + + + + 0 + + + + + + + + + 1 0 0 + + + + 0 + + + + + + + + 0 1 0 + + + + 0 + + + + + + + + 0 0 1 + + + + 0 + + + + + r_humerus + 0.0061 -0.2904 -0.0123 + 0 0 0 + 0 0 0 + 0 0 0 + + + + + + rotational + + 0 + + 0 + + 0 2.26892803 + + false + + false + + + + + + + false + + + + + + + + + arm_r_ulna.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_radius.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_lunate.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_scaphoid.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_pisiform.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_triquetrum.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_capitate.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_trapezium.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_trapezoid.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_hamate.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_1mc.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_2mc.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_3mc.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_4mc.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_5mc.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_thumbprox.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_thumbdist.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_2proxph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_2midph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_2distph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_3proxph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_3midph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_3distph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_4proxph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_4midph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_4distph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_5proxph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_5midph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + arm_r_5distph.vtp + + 1 1 1 + + + + -0 0 -0 0 0 0 + + 1 1 1 + + 4 + + 1 + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + + + + + + + + + + + + + + + + false + + 0 + + 1 + + + + + + -0.05365 -0.01373 0.14723 + base + + + -0.02714 -0.11441 -0.00664 + r_humerus + + + -0.03184 -0.22637 -0.01217 + r_humerus + + + -0.01743 -0.26757 -0.01208 + r_humerus + + + -0.0219 0.01046 -0.00078 + r_ulna_radius_hand + + + + + + + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + TRI + hybrid + -1 -1 + + + TRIlonghh + hybrid + -1 -1 + + + TRIlongglen + hybrid + -1 -1 + + + + + + + 1 + + 798.52 + + 0.134 + + 0.143 + + 0.20943951 + + 10 + + 0.01 + + 0.04 + + 0.033 + + 0.6 + + 0.5 + + 4 + + 0.3 + + 1.8 + + + + false + + 0 + + 1 + + + + + + -0.00599 -0.12646 0.00428 + r_humerus + + + -0.02344 -0.14528 0.00928 + r_humerus + + + -0.03184 -0.22637 -0.01217 + r_humerus + + + -0.01743 -0.26757 -0.01208 + r_humerus + + + -0.0219 0.01046 -0.00078 + r_ulna_radius_hand + + + + + + + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + TRI + hybrid + -1 -1 + + + + + + + 1 + + 624.3 + + 0.1138 + + 0.098 + + 0.15707963 + + 10 + + 0.01 + + 0.04 + + 0.033 + + 0.6 + + 0.5 + + 4 + + 0.3 + + 1.8 + + + + false + + 0 + + 1 + + + + + + -0.00838 -0.13695 -0.00906 + r_humerus + + + -0.02601 -0.15139 -0.0108 + r_humerus + + + -0.03184 -0.22637 -0.01217 + r_humerus + + + -0.01743 -0.26757 -0.01208 + r_humerus + + + -0.0219 0.01046 -0.00078 + r_ulna_radius_hand + + + + + + + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + TRI + hybrid + -1 -1 + + + + + + + 1 + + 624.3 + + 0.1138 + + 0.0908 + + 0.15707963 + + 10 + + 0.01 + + 0.04 + + 0.033 + + 0.6 + + 0.5 + + 4 + + 0.3 + + 1.8 + + + + false + + 0 + + 1 + + + + + + -0.039235 0.00347 0.14795 + base + + + -0.028945 0.01391 0.15639 + base + + + 0.02131 0.01793 0.01028 + r_humerus + + + 0.02378 -0.00511 0.01201 + r_humerus + + + 0.01345 -0.02827 0.00136 + r_humerus + + + 0.01068 -0.07736 -0.00165 + r_humerus + + + 0.01703 -0.12125 0.00024 + r_humerus + + + 0.0228 -0.1754 -0.0063 + r_humerus + + + 0.00751 -0.04839 0.02179 + r_ulna_radius_hand + + + + + + + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + BIClonghh + hybrid + 2 3 + + + + + + + 1 + + 624.3 + + 0.1157 + + 0.2723 + + 0 + + 10 + + 0.01 + + 0.04 + + 0.033 + + 0.6 + + 0.5 + + 4 + + 0.3 + + 1.8 + + + + false + + 0 + + 1 + + + + + + 0.004675 -0.01231 0.13475 + base + + + -0.007075 -0.04004 0.14507 + base + + + 0.01117 -0.07576 -0.01101 + r_humerus + + + 0.01703 -0.12125 -0.01079 + r_humerus + + + 0.0228 -0.1754 -0.0063 + r_humerus + + + 0.00751 -0.04839 0.02179 + r_ulna_radius_hand + + + + + + + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + + + + 1 + + 435.56 + + 0.1321 + + 0.1923 + + 0 + + 10 + + 0.01 + + 0.04 + + 0.033 + + 0.6 + + 0.5 + + 4 + + 0.3 + + 1.8 + + + + false + + 0 + + 1 + + + + + + 0.0068 -0.1739 -0.0036 + r_humerus + + + -0.0032 -0.0239 0.0009 + r_ulna_radius_hand + + + + + + + + + + + + 1 1 1 + + -0 0 -0 0 0 0 + + false + + 4 + + + + + TRI + hybrid + -1 -1 + + + + + + + 1 + + 987.26 + + 0.0858 + + 0.0535 + + 0 + + 10 + + 0.01 + + 0.04 + + 0.033 + + 0.6 + + 0.5 + + 4 + + 0.3 + + 1.8 + + + + + + + + + + base + + -0.01256 0.04 0.17 + + false + + + + r_humerus + + 0.005 -0.2904 0.03 + + false + + + + r_ulna_radius_hand + + -0.0011 -0.23559 0.0943 + + false + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/Applications/FunctionBasedPathConversion/test/testFunctionBasedPathConversion.cpp b/Applications/FunctionBasedPathConversion/test/testFunctionBasedPathConversion.cpp new file mode 100644 index 0000000000..8a6c6ef123 --- /dev/null +++ b/Applications/FunctionBasedPathConversion/test/testFunctionBasedPathConversion.cpp @@ -0,0 +1,330 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: testFunctionBasedPathConversion.cpp * + * -------------------------------------------------------------------------- * + * ScapulothoracicJoint is offered as an addition to the OpenSim API * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * * + * OpenSim is developed at Stanford University and is supported by: * + * * + * - The National Institutes of Health (U54 GM072970, R24 HD065690) * + * - DARPA, through the Warrior Web program * + * - The Chan Zuckerberg Initiative (CZI 2020-218896) * + * * + * Copyright (c) 2005-2021 Stanford University, TU Delft, and the Authors * + * Author(s): Joris Verhagen * + * * + * 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 +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace OpenSim; + +// output reporter that just appends some double output to an output vector +class VectorReporter final : public AbstractReporter { +public: + struct Datapoint { double time; double v; }; +private: + const Output& m_Source; + std::vector& m_Sink; + +public: + VectorReporter(const Output& source, + std::vector& sink) : + AbstractReporter{}, + m_Source{source}, + m_Sink{sink} + { + } + + void implementReport(const SimTK::State& s) const override { + double t = s.getTime(); + double v = m_Source.getValue(s); + + m_Sink.push_back(Datapoint{t, v}); + } + + VectorReporter* clone() const override { + return new VectorReporter{*this}; + } + + const std::string& getConcreteClassName() const override { + static std::string name = "VectorReporter"; + return name; + } +}; + +static void testLengthDifferenceBetweenFBPAndPBPIsSmall() { +// std::string inputModelPath = "ToyLandingModel.osim"; +// std::string outputModelName = "ToyLandingModel_FBP.osim"; +// std::string force = "/forceset/per_long_r"; +// std::string output = "length"; + + std::string inputModelPath = "arm26.osim"; + std::string outputModelName = "arm26_FBP.osim"; + std::string force = "/forceset/TRIlong"; + std::string output = "length"; + +// std::string inputModelPath = "SoccerKickingModel.osim"; +// std::string outputModelName = "SoccerKickingModel_FBP.osim"; +// std::string force = "/forceset/bifemlh_r"; +// std::string output = "length"; + + double reportingInterval = 0.05; + int discretizationPoints = 10; + + FunctionBasedPathConversionTool tool{inputModelPath, outputModelName}; + auto params = tool.getFittingParams(); + params.numDiscretizationStepsPerDimension = discretizationPoints; +// params.maxCoordsThatCanAffectPath = 2; + tool.setFittingParams(params); + tool.setVerbose(true); + tool.run(); + + // load + init both models + Model inputModel{inputModelPath}; + inputModel.finalizeConnections(); + inputModel.finalizeFromProperties(); + inputModel.initSystem(); + + Model outputModel{outputModelName}; + outputModel.finalizeConnections(); + outputModel.finalizeFromProperties(); + outputModel.initSystem(); + + // connect input model's force output to a vector that records the data + std::vector pbpDatapoints; + { + const AbstractOutput& ao = inputModel.getComponent(force + "/pointbasedpath").getOutput(output); + const Output* o = dynamic_cast*>(&ao); + if (!o) { + throw std::runtime_error{"Input model output is not of `double` type"}; + } + auto reporter = std::unique_ptr(new VectorReporter{*o, pbpDatapoints}); + reporter->set_report_time_interval(reportingInterval); + inputModel.addComponent(reporter.release()); + } + + // connect output model's force output to a vector that records the data + std::vector fbpDatapoints; + { + const AbstractOutput& ao = outputModel.getComponent(force + "/functionbasedpath").getOutput(output); + const Output* o = dynamic_cast*>(&ao); + if (!o) { + throw std::runtime_error{"Input model output is not of `double` type"}; + } + auto reporter = std::unique_ptr(new VectorReporter{*o, fbpDatapoints}); + reporter->set_report_time_interval(reportingInterval); + outputModel.addComponent(reporter.release()); + } + + // init initial system + states + double finalSimTime = 1.0; + + // run FD sim of PBP + { + SimTK::State& inputModelState = inputModel.initSystem(); + OpenSim::Manager manager{inputModel}; + manager.initialize(inputModelState); + manager.integrate(finalSimTime); + } + + // run FD sim of FBP + { + SimTK::State& outputModelState = outputModel.initSystem(); + OpenSim::Manager manager{outputModel}; + manager.initialize(outputModelState); + manager.integrate(finalSimTime); + } + + // validate API assumptions + size_t expectedSteps = static_cast(finalSimTime / reportingInterval); + { + OPENSIM_THROW_IF(pbpDatapoints.empty(), OpenSim::Exception, "pbpDatapoints empty: is it connected to the model?"); + OPENSIM_THROW_IF(fbpDatapoints.empty(), OpenSim::Exception, "fbpDatapoints empty: is it connected to the model?"); + OPENSIM_THROW_IF(pbpDatapoints.size() == expectedSteps, OpenSim::Exception, "pbpDatapoints has incorrect number of datapoints"); + OPENSIM_THROW_IF(fbpDatapoints.size() == expectedSteps, OpenSim::Exception, "fbpDatapoints has incorrect number of datapoints"); + + for (size_t i = 0; i < expectedSteps; ++i) { + double pbpT = pbpDatapoints[i].time; + double fbpT = fbpDatapoints[i].time; + OPENSIM_THROW_IF(!SimTK::isNumericallyEqual(pbpT, fbpT), OpenSim::Exception, "timepoints in PBP and FBP arrays differ"); + } + } + + struct RelErrStats { + double min = std::numeric_limits::max(); + double max = std::numeric_limits::min(); + double avg = 0.0; + int n = 0; + }; + RelErrStats stats; + + std::cout << "t \tpbp \tfbp \t%err \n"; + for (size_t i = 0; i < expectedSteps; ++i) { + double t = pbpDatapoints[i].time; + double pbpV = pbpDatapoints[i].v; + double fbpV = fbpDatapoints[i].v; + double relErr = (fbpV-pbpV)/pbpV; + double relPctErr = 100.0 * relErr; + + stats.min = std::min(stats.min, relErr); + stats.max = std::max(stats.max, relErr); + stats.avg = std::fma(stats.avg, stats.n, relErr) / (stats.n+1); + stats.n += 1; + + std::cout << std::fixed << std::setprecision(5) << t << '\t' << pbpV << '\t' << fbpV << '\t' << relPctErr << " %\n"; + } + + std::cout << "min = " << 100.0*stats.min << " %, max = " << 100.0*stats.max << " %, avg = " << 100.0*stats.avg << " %\n"; +} + +static void testArmModelConversionAccuracy() { +// std::string inputModelPath = "ToyLandingModel.osim"; +// std::string outputModelName = "ToyLandingModel_FBP.osim"; +// std::string force = "/forceset/per_long_r"; +// std::string output = "length"; + + std::string inputModelPath = "arm26.osim"; + std::string outputModelName = "arm26_FBP.osim"; + std::string force = "/forceset/TRIlong"; + std::string output = "length"; + + // run the function-based path (FBP) conversion tool to create an FBP-based output + FunctionBasedPathConversionTool tool{inputModelPath, outputModelName}; + + auto params = tool.getFittingParams(); + params.numDiscretizationStepsPerDimension = 10; +// params.maxCoordsThatCanAffectPath = 2; + tool.setFittingParams(params); + tool.setVerbose(true); + tool.run(); + + // load both the input and output into memory + Model inputModel{inputModelPath}; + Model outputModel{outputModelName}; + + // init the input model + inputModel.finalizeConnections(); + inputModel.finalizeFromProperties(); + inputModel.initSystem(); + + std::cout << "----- input model details -----\n"; + inputModel.printSubcomponentInfo(); + + // init the output model + outputModel.finalizeConnections(); + outputModel.finalizeFromProperties(); + outputModel.initSystem(); + + std::cout << "----- output model details -----\n"; + outputModel.printSubcomponentInfo(); + + // connect reporter to input model + if (true) { + auto inputModelReporter = std::unique_ptr{new ConsoleReporter{}}; + inputModelReporter->setName("point_results"); + inputModelReporter->set_report_time_interval(0.05); + inputModelReporter->addToReport(inputModel.getComponent(force + "/pointbasedpath").getOutput(output)); + inputModel.addComponent(inputModelReporter.release()); + } + + // connect reporter to output model + if (true) { + auto outputModelReporter = std::unique_ptr{new ConsoleReporter{}}; + outputModelReporter ->setName("function_results"); + outputModelReporter ->set_report_time_interval(0.05); + outputModelReporter ->addToReport(outputModel.getComponent(force + "/functionbasedpath").getOutput(output)); + outputModel.addComponent(outputModelReporter.release()); + } + + // init physical system + states + SimTK::State& inputModelState = inputModel.initSystem(); + SimTK::State& outputModelState = outputModel.initSystem(); + + // these are accumulated by each test run + struct TestStats { + std::chrono::high_resolution_clock::duration simTime{0}; + int stepsAttempted = 0; + int stepsTaken = 0; + std::string name; + + TestStats(std::string name_) : name{name_} {} + }; + + // function that runs a single test + auto runTest = [](OpenSim::Model& model, SimTK::State& state, TestStats& stats, double finalSimTime) { + OpenSim::Manager manager{model}; + manager.initialize(state); + + auto before = std::chrono::high_resolution_clock::now(); + manager.integrate(finalSimTime); + auto after = std::chrono::high_resolution_clock::now(); + + stats.simTime += after - before; + stats.stepsAttempted += manager.getIntegrator().getNumStepsAttempted(); + stats.stepsTaken += manager.getIntegrator().getNumStepsTaken(); + }; + + // setup + run the tests + TestStats inputStats{"Input Model (point-based paths)"}; + TestStats outputStats{"Output Model (function-based paths)"}; + int numRepeats = 25; + double finalSimTime = 1.0; + + // exercise input model + std::cerr << "----- running input model (point based path) tests -----\n"; + for (int i = 0; i < numRepeats; ++i) { + runTest(inputModel, inputModelState, inputStats, finalSimTime); + } + + // exercise output model + std::cerr << "----- running output model (function based path) tests -----\n"; + for (int i = 0; i < numRepeats; ++i) { + runTest(outputModel, outputModelState, outputStats, finalSimTime); + } + + // average out and print the stats + for (auto ts : {inputStats, outputStats}) { + ts.simTime /= numRepeats; + ts.stepsAttempted /= numRepeats; + ts.stepsTaken /= numRepeats; + + long millis = static_cast(std::chrono::duration_cast(ts.simTime).count()); + + printf("%s: \n avg. time (ms) = %ld \n integration steps attempted = %i \n integration steps taken = %i)\n", + ts.name.c_str(), + millis, + ts.stepsAttempted, + ts.stepsTaken); + } +} + +int main() { + try { + SimTK_START_TEST("testFunctionBasedPathConversion"); + SimTK_SUBTEST(testLengthDifferenceBetweenFBPAndPBPIsSmall); + SimTK_SUBTEST(testArmModelConversionAccuracy); + SimTK_END_TEST(); + } catch (const std::exception& ex) { + OpenSim::log_error(ex.what()); + } +} diff --git a/Bindings/OpenSimHeaders_simulation.h b/Bindings/OpenSimHeaders_simulation.h index f22f948d4c..7138d9c5b0 100644 --- a/Bindings/OpenSimHeaders_simulation.h +++ b/Bindings/OpenSimHeaders_simulation.h @@ -109,7 +109,11 @@ #include #include #include + #include +#include +#include + #include #include diff --git a/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp b/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp index 5d68506aad..43d376d35e 100644 --- a/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp +++ b/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp @@ -81,7 +81,6 @@ void Blankevoort1991Ligament::setNull() } void Blankevoort1991Ligament::constructProperties() { - constructProperty_GeometryPath(GeometryPath()); constructProperty_linear_stiffness(1.0); constructProperty_transition_strain(0.06); constructProperty_damping_coefficient(0.003); diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp new file mode 100644 index 0000000000..5f2a08958a --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -0,0 +1,988 @@ +#include "FunctionBasedPath.h" + +#include +#include +#include +#include + +#include +#include +#include + +static constexpr size_t g_MaxCoordsThatCanBeInterpolated = 8; // important: this is an upper limit that's used for stack allocations +static constexpr int g_MaxCoordsThatCanAffectPathDefault = static_cast(g_MaxCoordsThatCanBeInterpolated); +static constexpr int g_NumProbingDiscretizationsDefault = 8; +static constexpr double g_MinProbingMomentArmChangeDefault = 0.001; +static constexpr int g_NumDiscretizationStepsPerDimensionDefault = 8; + +// returns `true` if changing the supplied `Coordinate` changes the moment arm +// of the supplied `PointBasedPath` (PBP) +static bool coordAffectsPBP( + OpenSim::PointBasedPath const& pbp, + OpenSim::Coordinate const& c, + SimTK::State& state, + int numProbingSteps, + double minMomentArmChangeRequired) { + + bool initialLockedState = c.getLocked(state); + double initialValue = c.getValue(state); + + c.setLocked(state, false); + + double start = c.getRangeMin(); + double end = c.getRangeMax(); + double step = (end - start) / (numProbingSteps-1); + + bool affectsCoord = false; + for (double v = start; v <= end; v += step) { + c.setValue(state, v); + double ma = pbp.computeMomentArm(state, c); + + if (std::abs(ma) >= minMomentArmChangeRequired) { + affectsCoord = true; + break; + } + } + + c.setValue(state, initialValue); + c.setLocked(state, initialLockedState); + + return affectsCoord; +} + +// returns a sequence of `OpenSim::Coordinate`s that affect the supplied +// point-based path (PBP) +// +// is not guaranteed to find *all* coordinates that affect the supplied PBP, +// because that may involve extreme probing (which this implementation does not +// do) +static std::vector coordsThatAffectPBP(OpenSim::Model const& model, + OpenSim::PointBasedPath const& pbp, + SimTK::State& st, + int numProbingSteps, + double minMomentArmChangeRequired) { + std::vector rv; + for (OpenSim::Coordinate const& c : model.getComponentList()){ + if (c.getMotionType() == OpenSim::Coordinate::MotionType::Coupled) { + continue; + } + + if (!coordAffectsPBP(pbp, c, st, numProbingSteps, minMomentArmChangeRequired)) { + continue; + } + + rv.push_back(&c); + } + return rv; +} + +// discretization of a particular coordinate +// +// assumes `nsteps` evenly-spaced points ranging from [begin, end] (inclusive) +struct Discretization final { + double begin; + double end; + int nsteps; +}; + +// compute ideal discretization of the given coordinate +static Discretization discretizationForCoord(OpenSim::Coordinate const& c, int numDiscretizationSteps) { + SimTK_ASSERT_ALWAYS(numDiscretizationSteps >= 4, "need to supply more than 4 discretization steps"); + + Discretization d; + //d.begin = -static_cast(SimTK_PI)/2; + //d.end = static_cast(SimTK_PI)/2; + d.begin = std::max(c.getRangeMin(), -static_cast(SimTK_PI)); + d.end = std::min(c.getRangeMax(), static_cast(SimTK_PI)); + d.nsteps = numDiscretizationSteps - 3; + double step = (d.end-d.begin) / (d.nsteps-1); + + // expand range slightly in either direction to ensure interpolation is + // clean around the edges + d.begin -= step; + d.end += 2.0 * step; + d.nsteps += 3; + + return d; +} + +// compute all permutations of the coordinates for the given discretizations +// +// these output values are stored "lexographically", with coords being "big endian", so: +// +// - [coord[0].begin, coord[1].begin, ..., coord[n-1].begin] +// - [coord[0].begin, coord[1].begin, ..., (coord[n-1].begin + step)] +// - [coord[0].begin, coord[1].begin, ..., (coord[n-1].begin + 2*step)] +// - ... +// - [coord[0].begin, (coord[1].begin + step), ..., coord[n-1].begin] +// - [coord[0].begin, (coord[1].begin + step), ..., (coord[n-1].begin + step)] +// - ... +// - [(coord[0].begin + step), coord[1].begin, ..., coord[n-1].begin] +// - [(coord[0].begin + step), coord[1].begin, ..., (coord[n-1].begin + step)] +static std::vector computeEvaluationsFromPBP(OpenSim::PointBasedPath const& pbp, + SimTK::State& state, + OpenSim::Coordinate const** coords, + Discretization const* discs, + size_t ncoords) { + std::vector rv; + + if (ncoords == 0) { // edge-case: logic below assumes ncoords > 0 + return rv; + } + + OPENSIM_THROW_IF(ncoords > g_MaxCoordsThatCanBeInterpolated, OpenSim::Exception, "too many coordinates affect this path - the FunctionBasedPath implementation cannot handle this"); + + // number of evaluations is the total number of permutations of all dimensions for + // all discretizations + int expectedEvals = 1; + for (size_t i = 0; i < ncoords; ++i) { + expectedEvals *= discs[i].nsteps; + } + rv.reserve(expectedEvals); + + // holds which "step" in each Coordinate's [begin, end] discretization we + // have evaluated up to + std::array discStepIdx{}; + while (discStepIdx[0] < discs[0].nsteps) { + + // set all coordinate values for this step + for (size_t coord = 0; coord < ncoords; ++coord) { + Discretization const& discr = discs[coord]; + + double stepSz = (discr.end - discr.begin) / (discr.nsteps - 1); + int step = discStepIdx[coord]; + double val = discr.begin + step*stepSz; + + coords[coord]->setValue(state, val); + } + + // eval the length of the PBP for this permutation of coordinate values + { + double eval = pbp.getLength(state); + rv.push_back(eval); + } + + // update which coordinate steps we're up to for each coordinate + // + // always updates "least significant" coordinate first, then performs + // "carry propagation" to the "more significant" coordinates + int pos = ncoords - 1; + discStepIdx[pos]++; + while (pos > 0 && discStepIdx[pos] >= discs[pos].nsteps) { + discStepIdx[pos] = 0; // overflow + ++discStepIdx[pos-1]; // carry + --pos; + } + } + + SimTK_ASSERT_ALWAYS(discStepIdx[0] == discs[0].nsteps, "should be true, after the final overflow"); + for (size_t i = 1; i < discStepIdx.size(); ++i) { + SimTK_ASSERT_ALWAYS(discStepIdx[i] == 0, "these less-significant coordinates should all be overflow-n by the end of the alg"); + } + SimTK_ASSERT_ALWAYS(rv.size() == static_cast(expectedEvals), "these two values should match, given the above alg"); + + return rv; +} + +struct OpenSim::FunctionBasedPath::Impl final { + // direct pointers to each coordinate + std::vector coords; + + // absolute paths of each coordinate (1:1 with coords) + std::vector coordAbsPaths; + + // discretizations ranges for each coordinate (1:1 with coords) + std::vector discretizations; + + // evaluations for each permutation of coordinates' discretizations + std::vector evals; + + // satisfies clone-ability requirement of SimTK::ClonePtr + Impl* clone() const { + auto p = std::unique_ptr{new Impl{*this}}; + return p.release(); + } +}; + +// compute fresh implementation data from an existing PointBasedPath by +// evaluating it and fitting it to a function-based curve +// +// returns false if too many/too little coordinates affect the path +static bool Impl_ComputeFromPBP(OpenSim::FunctionBasedPath::Impl& impl, + const OpenSim::Model& model, + const OpenSim::PointBasedPath& pbp, + const OpenSim::FunctionBasedPath::FittingParams& params) { + + // copy model, so we can independently equilibrate + realize + modify the + // copy without having to touch the source model + std::unique_ptr modelClone{model.clone()}; + SimTK::State& initialState = modelClone->initSystem(); + modelClone->equilibrateMuscles(initialState); + modelClone->realizeVelocity(initialState); + + // set `coords` + impl.coords = coordsThatAffectPBP(*modelClone, pbp, initialState, params.numProbingDiscretizations, params.minProbingMomentArmChange); + if (static_cast(impl.coords.size()) > params.maxCoordsThatCanAffectPath || impl.coords.size() == 0) { + impl.coords.clear(); + return false; + } + + // set `coordAbsPaths` + impl.coordAbsPaths.clear(); + impl.coordAbsPaths.reserve(impl.coords.size()); + for (const OpenSim::Coordinate* c : impl.coords) { + impl.coordAbsPaths.push_back(c->getAbsolutePathString()); + } + + // set `discretizations` + impl.discretizations.clear(); + impl.discretizations.reserve(impl.coords.size()); + for (const OpenSim::Coordinate* c : impl.coords) { + impl.discretizations.push_back(discretizationForCoord(*c, params.numDiscretizationStepsPerDimension)); + } + + // set `evals` + SimTK_ASSERT_ALWAYS(impl.coords.size() == impl.discretizations.size(), "these should be equal by now"); + impl.evals = computeEvaluationsFromPBP(pbp, initialState, impl.coords.data(), impl.discretizations.data(), impl.coords.size()); + + return true; +} + +// init underlying implementation data from a `FunctionBasedPath`s (precomputed) properties +// +// the properties being set in the FBP usually implies that the FBP has already been built +// from a PBP at some previous point in time +static void Impl_InitFromFBPProperties(OpenSim::FunctionBasedPath::Impl& impl, + OpenSim::FunctionBasedPath const& fbp) { + + OpenSim::FunctionBasedPathDiscretizationSet const& discSet = fbp.getProperty_FunctionBasedPathDiscretizationSet().getValue(); + + // set `coords` pointers to null + // + // they are lazily looked up in a later phase (after the model is connected up) + impl.coords.clear(); + impl.coords.resize(discSet.getSize(), nullptr); + + // set `coordAbsPaths` from discretizations property + impl.coordAbsPaths.clear(); + impl.coordAbsPaths.reserve(discSet.getSize()); + for (int i = 0; i < discSet.getSize(); ++i) { + impl.coordAbsPaths.push_back(discSet[i].getProperty_coordinate_abspath().getValue()); + } + + // set `discretizations` from discretizations property + impl.discretizations.clear(); + impl.discretizations.reserve(discSet.getSize()); + for (int i = 0; i < discSet.getSize(); ++i) { + OpenSim::FunctionBasedPathDiscretization const& disc = discSet[i]; + Discretization d; + d.begin = disc.getProperty_x_begin().getValue(); + d.end = disc.getProperty_x_end().getValue(); + d.nsteps = disc.getProperty_num_points().getValue(); + impl.discretizations.push_back(d); + } + + // set `evals` from evaluations property + auto const& evalsProp = fbp.getProperty_Evaluations(); + impl.evals.clear(); + impl.evals.reserve(evalsProp.size()); + for (int i = 0; i < evalsProp.size(); ++i) { + impl.evals.push_back(evalsProp.getValue(i)); + } +} + +// ensure that the OpenSim::Coordinate* pointers held in Impl are up-to-date +// +// the pointers are there to reduce runtime path lookups +static void Impl_SetCoordinatePointersFromCoordinatePaths(OpenSim::FunctionBasedPath::Impl& impl, + OpenSim::Component const& c) { + + for (size_t i = 0; i < impl.coords.size(); ++i) { + impl.coords[i] = &c.getComponent(impl.coordAbsPaths[i]); + } +} + +// returns interpolated path length for a given permutation of coordinate +// values +// +// this is the "heart" of the FPB algorithm. It's loosely based on the algorithm +// described here: +// +// "Two hierarchies of spline interpolations. Practical algorithms for multivariate higher order splines" +// https://arxiv.org/abs/0905.3564 +// +// `inputVals` points to a sequence of `nCoords` values that were probably +// retrieved via `Coordinate::getValue(SimTK::State const&)`. The reason +// that `inputVals` is provided externally (rather than have this implementation +// handle calling `getValue`) is because derivative calculations need to fiddle +// the input values slightly +static double Impl_GetPathLength(OpenSim::FunctionBasedPath::Impl const& impl, + double const* inputVals, + int nCoords) { + + SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); + SimTK_ASSERT_ALWAYS(nCoords == static_cast(impl.coords.size()), "You must call this function with the correct number of (precomputed) coordinate values"); + + // compute: + // + // - the index of the first discretization step *before* the input value + // + // - the polynomial of the curve at that step, given its fractional distance + // toward the next step + using Polynomial = std::array; + std::array closestDiscretizationSteps; + std::array betas; + for (int coord = 0; coord < nCoords; ++coord) { + double inputVal = inputVals[coord]; + Discretization const& disc = impl.discretizations[coord]; + double step = (disc.end - disc.begin) / (disc.nsteps - 1); + + // compute index of first discretization step *before* the input value and + // the fraction that the input value is towards the *next* discretization step + int idx; + double frac; + if (inputVal < disc.begin+step) { + idx = 1; + frac = 0.0; + } else if (inputVal > disc.end-2*step) { + idx = disc.nsteps-3; + frac = 0.0; + } else { + // solve for `n`: inputVal = begin + n*step + double n = (inputVal - disc.begin) / step; + double wholePart; + double fractionalPart = std::modf(n, &wholePart); + + idx = static_cast(wholePart); + frac = fractionalPart; + } + + // compute polynomial based on fraction the point is toward the next point + double frac2 = frac*frac; + double frac3 = frac2*frac; + double frac4 = frac3*frac; + double fracMinusOne = frac - 1; + double fracMinusOne3 = fracMinusOne*fracMinusOne*fracMinusOne; + + Polynomial p; + p[0] = 0.5 * fracMinusOne3*frac*(2*frac + 1); + p[1] = -0.5 * (frac - 1)*(6*frac4 - 9*frac3 + 2*frac + 2); + p[2] = 0.5 * frac*(6*frac4 - 15*frac3 + 9*frac2 + frac + 1); + p[3] = -0.5 * (frac - 1)*frac3*(2*frac - 3); + + closestDiscretizationSteps[coord] = idx; + betas[coord] = p; + } + + // for each coord, permute through 4 locations *around* the input's location: + // + // - A one step before B + // - B the first discretization step before the input value + // - C one step after B + // - D one step after C + // + // where: + // + // - betas are coefficients that affect each location. beta[0] affects A, + // betas[1] affects B, betas[2] affects C, and betas[3] affects D + + // represent permuting through each location around each coordinate as a string + // of integer offsets that can be -1, 0, 1, or 2 + // + // the algorithm increments this array as it goes through each permutation + std::array dimIdxOffsets; + for (int coord = 0; coord < nCoords; ++coord) { + dimIdxOffsets[coord] = -1; + } + + // permute through all locations around the input value + // + // e.g. the location permutations for 3 coords iterate like this for each + // crank of the loop + // + // [-1, -1, -1] + // [-1, -1, 0] + // [-1, -1, 1] + // [-1, -1, 2] + // [-1, 0, -1] + // ...(4^3 steps total)... + // [ 2, 2, 1] + // [ 2, 2, 2] + // [ 3, 0, 0] (the termination condition) + + double z = 0.0; + int cnt = 0; + while (dimIdxOffsets[0] < 3) { + + // compute `beta` (weighted coefficient per coord) for this particular + // permutation's coordinate locations (e.g. -1, 0, 0, 2) and figure out + // what the closest input value was at the weighted location. Add the + // result the the output + + double beta = 1.0; + int evalStride = 1; + int evalIdx = 0; + + // go backwards, from least-significant coordinate (highest idx) + // + // this is so that we can compute the stride as the algorithm runs + for (int coord = nCoords-1; coord >= 0; --coord) { + int offset = dimIdxOffsets[coord]; // -1, 0, 1, or 2 + int closestStep = closestDiscretizationSteps[coord]; + int step = closestStep + offset; + + beta *= betas[coord][offset+1]; + evalIdx += evalStride * step; + evalStride *= impl.discretizations[coord].nsteps; + } + + // equivalent to z += b*v, but handles rounding errors when the rhs + // is very small + z = std::fma(beta, impl.evals.at(evalIdx), z); + + // increment the offsets + // + // this is effectively the step that permutes [-1, -1, 2] --> [-1, 0, -1] + { + int pos = nCoords-1; + ++dimIdxOffsets[pos]; // perform least-significant increment (may overflow) + while (pos > 0 && dimIdxOffsets[pos] > 2) { // handle overflows + carry propagation + dimIdxOffsets[pos] = -1; // overflow + ++dimIdxOffsets[pos-1]; // carry propagation + --pos; + } + } + + ++cnt; + } + + // sanity check: is `z` accumulated from the expected number of iterations? + { + int expectedIterations = 1 << (2*nCoords); + if (cnt != expectedIterations) { + std::stringstream msg; + msg << "invalid number of permutations explored: expected = " << expectedIterations << ", got = " << cnt; + OPENSIM_THROW(OpenSim::Exception, std::move(msg).str()); + } + } + + return z; +} + +// get the length of the path in the current state +static double Impl_GetPathLength(OpenSim::FunctionBasedPath::Impl const& impl, + SimTK::State const& s) { + + int nCoords = static_cast(impl.coords.size()); + + // get the input value of each coordinate in the current state + std::array inputVals{}; + for (int coord = 0; coord < nCoords; ++coord) { + inputVals[coord] = impl.coords[coord]->getValue(s); + } + + return Impl_GetPathLength(impl, inputVals.data(), nCoords); +} + +// get the *derivative* of the path length with respect to the given Coordinate index +// (in impl.coords) +//static double Impl_GetPathLengthDerivative(OpenSim::FunctionBasedPath::Impl const& impl, +// SimTK::State const& s, +// int coordIdx) { + +// SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); +// SimTK_ASSERT_ALWAYS(coordIdx != -1, "coord index must be valid"); +// SimTK_ASSERT_ALWAYS(coordIdx < static_cast(impl.coords.size()), "coord index must be valid"); + +// int nCoords = static_cast(impl.coords.size()); + +// // get the input value of each coordinate in the current state +// std::array inputVals{}; +// for (int coord = 0; coord < nCoords; ++coord) { +// inputVals[coord] = impl.coords[coord]->getValue(s); +// } + +// // compute value at current point +// double v1 = Impl_GetPathLength(impl, inputVals.data(), nCoords); + +// static constexpr double h = 0.00001; + +// // alter the input value for the to-be-derived coordinate *slightly* and recompute +// inputVals[coordIdx] += h; +// double v2 = Impl_GetPathLength(impl, inputVals.data(), nCoords); + +// // the derivative is how much the output changed when the input was altered +// // slightly (this is a poor-man's discrete derivative method) +// return (v2 - v1) / h; +//} +static double Impl_GetPathLengthDerivative(OpenSim::FunctionBasedPath::Impl const& impl, + SimTK::State const& s, + int coordIdx) { + int nCoords = static_cast(impl.coords.size()); + + SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); + SimTK_ASSERT_ALWAYS(nCoords == static_cast(impl.coords.size()), "You must call this function with the correct number of (precomputed) coordinate values"); + + // get the input value of each coordinate in the current state + std::array inputVals{}; + for (int coord = 0; coord < nCoords; ++coord) { + inputVals[coord] = impl.coords[coord]->getValue(s); + } + + // compute: + // + // - the index of the first discretization step *before* the input value + // + // - the polynomial of the curve at that step, given its fractional distance + // toward the next step + using Polynomial = std::array; + std::array closestDiscretizationSteps; + std::array betas; + for (int coord = 0; coord < nCoords; ++coord) { + double inputVal = inputVals[coord]; + Discretization const& disc = impl.discretizations[coord]; + double step = (disc.end - disc.begin) / (disc.nsteps - 1); + + // compute index of first discretization step *before* the input value and + // the fraction that the input value is towards the *next* discretization step + int idx; + double frac; + if (inputVal < disc.begin+step) { + idx = 1; + frac = 0.0; + } else if (inputVal > disc.end-2*step) { + idx = disc.nsteps-3; + frac = 0.0; + } else { + // solve for `n`: inputVal = begin + n*step + double n = (inputVal - disc.begin) / step; + double wholePart; + double fractionalPart = std::modf(n, &wholePart); + + idx = static_cast(wholePart); + frac = fractionalPart; + } + + // compute polynomial based on fraction the point is toward the next point + double frac2 = frac*frac; + double frac3 = frac2*frac; + double frac4 = frac3*frac; + double fracMinusOne = frac - 1; + double fracMinusOne3 = fracMinusOne*fracMinusOne*fracMinusOne; + + Polynomial p; + if (coord == coordIdx){ + // derivative + p[0] = 5*frac4 - 10*frac3 + 4.5*frac2 + frac - 0.5; + p[1] = -15*frac4 + 30*frac3 - 13.5*frac2 - 2*frac; + p[2] = 15*frac4 - 30*frac3 + 13.5*frac2 + frac + 0.5; + p[3] = frac2*(-5*frac2 + 10*frac - 4.5); + } else { + // 'normal' spline function + p[0] = 0.5 * fracMinusOne3*frac*(2*frac + 1); + p[1] = -0.5 * (frac - 1)*(6*frac4 - 9*frac3 + 2*frac + 2); + p[2] = 0.5 * frac*(6*frac4 - 15*frac3 + 9*frac2 + frac + 1); + p[3] = -0.5 * (frac - 1)*frac3*(2*frac - 3); + } + + closestDiscretizationSteps[coord] = idx; + betas[coord] = p; + } + + std::array dimIdxOffsets; + for (int coord = 0; coord < nCoords; ++coord) { + dimIdxOffsets[coord] = -1; + } + + double z = 0.0; + int cnt = 0; + while (dimIdxOffsets[0] < 3) { + + double beta = 1.0; + int evalStride = 1; + int evalIdx = 0; + + for (int coord = nCoords-1; coord >= 0; --coord) { + int offset = dimIdxOffsets[coord]; // -1, 0, 1, or 2 + int closestStep = closestDiscretizationSteps[coord]; + int step = closestStep + offset; + + beta *= betas[coord][offset+1]; + evalIdx += evalStride * step; + evalStride *= impl.discretizations[coord].nsteps; + } + + double gridSize = (impl.discretizations[coordIdx].end-impl.discretizations[coordIdx].begin)/impl.discretizations[coordIdx].nsteps; + z = std::fma(beta, impl.evals.at(evalIdx)/gridSize, z); + { + int pos = nCoords-1; + ++dimIdxOffsets[pos]; // perform least-significant increment (may overflow) + while (pos > 0 && dimIdxOffsets[pos] > 2) { // handle overflows + carry propagation + dimIdxOffsets[pos] = -1; // overflow + ++dimIdxOffsets[pos-1]; // carry propagation + --pos; + } + } + + ++cnt; + } + { + int expectedIterations = 1 << (2*nCoords); + if (cnt != expectedIterations) { + std::stringstream msg; + msg << "invalid number of permutations explored: expected = " << expectedIterations << ", got = " << cnt; + OPENSIM_THROW(OpenSim::Exception, std::move(msg).str()); + } + } + + return z; +} + +// get the *derivative* of the path length with respect to the given Coordinate +static double Impl_GetPathLengthDerivative(OpenSim::FunctionBasedPath::Impl const& impl, + SimTK::State const& s, + OpenSim::Coordinate const& c) { + + // figure out the index of the coordinate being referred to + int coordIdx = -1; + for (int i = 0; i < static_cast(impl.coords.size()); ++i) { + if (impl.coords[i] == &c) { + coordIdx = i; + break; + } + } + + // ensure the coordinate was actually found, or this alg will break + if (coordIdx == -1) { + std::stringstream msg; + msg << "could not find coordiante '" << c.getName() << "' in the set of coordinates the FunctionBasedPath handles. Coordinates handled by this path are: "; + char const* delim = ""; + for (OpenSim::Coordinate const* c : impl.coords) { + msg << delim << c->getName(); + delim = ", "; + } + OPENSIM_THROW(OpenSim::Exception, std::move(msg).str()); + } + + // use the "raw" (non-lookup) version of this function with the index + return Impl_GetPathLengthDerivative(impl, s, coordIdx); +} + +// get the lengthening speed of the path in the current state +static double Impl_GetLengtheningSpeed(const OpenSim::FunctionBasedPath::Impl& impl, + const SimTK::State& state) { + + double lengtheningSpeed = 0.0; + for (int coordIdx = 0; coordIdx < static_cast(impl.coords.size()); ++coordIdx) { + double deriv = Impl_GetPathLengthDerivative(impl, state, coordIdx); + double coordSpeedVal = impl.coords[coordIdx]->getSpeedValue(state); + + lengtheningSpeed = std::fma(deriv, coordSpeedVal, lengtheningSpeed); + } + return lengtheningSpeed; +} + +//---------------------------------------------------------------------------- +// PUBLIC API +//---------------------------------------------------------------------------- + +OpenSim::FunctionBasedPath::FittingParams::FittingParams() : + maxCoordsThatCanAffectPath{g_MaxCoordsThatCanAffectPathDefault}, + numProbingDiscretizations{g_NumProbingDiscretizationsDefault}, + minProbingMomentArmChange{g_MinProbingMomentArmChangeDefault}, + numDiscretizationStepsPerDimension{g_NumDiscretizationStepsPerDimensionDefault} +{ +} + +std::unique_ptr OpenSim::FunctionBasedPath::fromPointBasedPath( + const Model& model, + const PointBasedPath& pbp, + FittingParams params) +{ + // sanitize + validate params + { + if (params.maxCoordsThatCanAffectPath == -1) { + params.maxCoordsThatCanAffectPath = g_MaxCoordsThatCanBeInterpolated; + } + + if (params.numProbingDiscretizations == -1) { + params.numProbingDiscretizations = g_NumProbingDiscretizationsDefault; + } + + if (params.minProbingMomentArmChange < 0.0) { + params.minProbingMomentArmChange = g_MinProbingMomentArmChangeDefault; + } + + if (params.numDiscretizationStepsPerDimension == -1) { + params.numDiscretizationStepsPerDimension = g_NumDiscretizationStepsPerDimensionDefault; + } + + OPENSIM_THROW_IF(params.maxCoordsThatCanAffectPath <= 0, OpenSim::Exception, "maxCoordsThatCanAffectPath must be a positive number that is <=8"); + OPENSIM_THROW_IF(params.maxCoordsThatCanAffectPath > static_cast(g_MaxCoordsThatCanBeInterpolated), OpenSim::Exception, "maxCoordsThatCanAffectPath must be a positive number that is <=8"); + OPENSIM_THROW_IF(params.numProbingDiscretizations <= 0, OpenSim::Exception, "numProbingDiscretizations must be a positive number"); + OPENSIM_THROW_IF(params.minProbingMomentArmChange <= 0, OpenSim::Exception, "minProbingMomentArmChange must be a positive number"); + OPENSIM_THROW_IF(params.numDiscretizationStepsPerDimension <= 0, OpenSim::Exception, "numDiscretizationStepsPerDimension must be a positive number"); + } + + std::unique_ptr fbp{new FunctionBasedPath{}}; + + // copy relevant data from source PBP into the output FBP + fbp->upd_Appearance() = pbp.get_Appearance(); + fbp->setPathPointSet(pbp.getPathPointSet()); + fbp->setPathWrapSet(pbp.getWrapSet()); + + OpenSim::FunctionBasedPath::Impl& impl = *fbp->_impl; + + // compute underlying impl data from the PBP + if (!Impl_ComputeFromPBP(impl, model, pbp, params)) { + return nullptr; + } + + // write impl discretizations into the `Discretizations` property + OpenSim::FunctionBasedPathDiscretizationSet& set = fbp->updProperty_FunctionBasedPathDiscretizationSet().updValue(); + for (size_t i = 0; i < impl.coords.size(); ++i) { + auto disc = std::unique_ptr{new FunctionBasedPathDiscretization{}}; + disc->set_x_begin(impl.discretizations[i].begin); + disc->set_x_end(impl.discretizations[i].end); + disc->set_num_points(impl.discretizations[i].nsteps); + disc->set_coordinate_abspath(impl.coordAbsPaths[i]); + set.adoptAndAppend(disc.release()); + } + + // write evals into `Evaluations` property + auto& evalsProp = fbp->updProperty_Evaluations(); + for (double eval : fbp->_impl->evals) { + evalsProp.appendValue(eval); + } + + return fbp; +} + +OpenSim::FunctionBasedPath::FunctionBasedPath() : GeometryPath{}, _impl{new Impl{}} +{ + constructProperty_FunctionBasedPathDiscretizationSet(FunctionBasedPathDiscretizationSet{}); + constructProperty_Evaluations(); +} +OpenSim::FunctionBasedPath::FunctionBasedPath(const FunctionBasedPath&) = default; +OpenSim::FunctionBasedPath::FunctionBasedPath(FunctionBasedPath&&) = default; +OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(const FunctionBasedPath&) = default; +OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(FunctionBasedPath&&) = default; +OpenSim::FunctionBasedPath::~FunctionBasedPath() noexcept = default; + +void OpenSim::FunctionBasedPath::extendFinalizeFromProperties() +{ + Impl_InitFromFBPProperties(*_impl, *this); +} + +void OpenSim::FunctionBasedPath::extendFinalizeConnections(OpenSim::Component& root) +{ + // populate pointer-based coordinate lookups + // + // the reason this isn't done in `extendFinalizeFromProperties` is because the + // not-yet-property-finalized Model hasn't necessarily "connected" to the + // coordinates that the coordinate files refer to, so the implementation + // can't lookup the `OpenSim::Coordinate*` pointers during that phase + + // Allow (model) component to include its own subcomponents + // before calling the base method which automatically invokes + // connect all the subcomponents. + { + Model* model = dynamic_cast(&root); + if (model) { + connectToModel(*model); + } + } + + Impl_SetCoordinatePointersFromCoordinatePaths(*_impl, root); +} + +double OpenSim::FunctionBasedPath::getLength(const SimTK::State& s) const +{ + if (isCacheVariableValid(s, _lengthCV)) { + return getCacheVariableValue(s, _lengthCV); + } + + double v = Impl_GetPathLength(*_impl, s); + setCacheVariableValue(s, _lengthCV, v); + return v; +} + +void OpenSim::FunctionBasedPath::setLength(const SimTK::State& s, double length) const +{ + setCacheVariableValue(s, _lengthCV, length); +} + +double OpenSim::FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const +{ + if (isCacheVariableValid(s, _speedCV)) { + return getCacheVariableValue(s, _speedCV); + } + + double v = Impl_GetLengtheningSpeed(*_impl, s); + setCacheVariableValue(s, _speedCV, v); + return v; +} + +void OpenSim::FunctionBasedPath::setLengtheningSpeed(const SimTK::State& s, double speed) const +{ + setCacheVariableValue(s, _speedCV, speed); +} + +double OpenSim::FunctionBasedPath::computeMomentArm(const SimTK::State& s, + const Coordinate& aCoord) const +{ + return Impl_GetPathLengthDerivative(*_impl, s, aCoord); +} + +/* add in the equivalent spatial forces on bodies for an applied tension + along the GeometryPath to a set of bodyForces */ +void OpenSim::FunctionBasedPath::addInEquivalentForces(const SimTK::State& s, + const double& tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const +{ + const SimTK::SimbodyMatterSubsystem& matter = getModel().getMatterSubsystem(); + + for (const OpenSim::Coordinate* coord : _impl->coords) { + double ma = computeMomentArm(s, *coord); + double torqueOverCoord = -tension*ma; + + matter.addInMobilityForce(s, + SimTK::MobilizedBodyIndex(coord->getBodyIndex()), + SimTK::MobilizerUIndex(coord->getMobilizerQIndex()), + torqueOverCoord, + mobilityForces); + } +} + +void OpenSim::FunctionBasedPath::generateDecorations( + bool fixed, + const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& appendToThis) const +{ + static bool shownOnce = []() { + OpenSim::log_warn("Tried to call `generateDecorations` on a `FunctionBasedPath`. This function call will be ignored (there is no easy way to visualize function-based paths)"); + return true; + }(); + (void)shownOnce; +} + + +void OpenSim::FunctionBasedPath::computePath(const SimTK::State& s) const +{ + OPENSIM_THROW(Exception, "Tried to call `computePath` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it isn't path-based). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + + +void OpenSim::FunctionBasedPath::computeLengtheningSpeed(const SimTK::State& s) const +{ + double lengtheningspeed = getLengtheningSpeed(s); + setLengtheningSpeed(s, lengtheningspeed); +} + +double OpenSim::FunctionBasedPath::calcLengthAfterPathComputation( + const SimTK::State& s, + const Array& currentPath) const +{ + return getLength(s); +} + +double OpenSim::FunctionBasedPath::calcPathLengthChange(const SimTK::State& s, + const WrapObject& wo, + const WrapResult& wr, + const Array& path) const +{ + OPENSIM_THROW(Exception, "Tried to call `calcPathLengthChange` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it isn't path-based). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +void OpenSim::FunctionBasedPath::getPointForceDirections(const SimTK::State& s, + OpenSim::Array *rPFDs) const +{ + OPENSIM_THROW(Exception, "Tried to call `getPointForceDirections` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it isn't path-based). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +const OpenSim::Array& OpenSim::FunctionBasedPath::getCurrentPath(const SimTK::State& s) const +{ + OPENSIM_THROW(Exception, "Tried to call `getCurrentPath` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain a path). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +OpenSim::AbstractPathPoint* OpenSim::FunctionBasedPath::addPathPoint( + const SimTK::State& s, + int index, + const PhysicalFrame& frame) +{ + OPENSIM_THROW(Exception, "Tried to call `addPathPoint` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain a path). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +OpenSim::AbstractPathPoint* OpenSim::FunctionBasedPath::appendNewPathPoint( + const std::string& proposedName, + const PhysicalFrame& frame, + const SimTK::Vec3& locationOnFrame) +{ + OPENSIM_THROW(Exception, "Tried to call `appendNewPathPoint` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain a path). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +bool OpenSim::FunctionBasedPath::canDeletePathPoint(int index) +{ + OPENSIM_THROW(Exception, "Tried to call `canDeletePathPoint` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain a path). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +bool OpenSim::FunctionBasedPath::deletePathPoint(const SimTK::State& s, + int index) +{ + OPENSIM_THROW(Exception, "Tried to call `deletePathPoint` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain a path). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +bool OpenSim::FunctionBasedPath::replacePathPoint(const SimTK::State& s, + AbstractPathPoint* oldPathPoint, + AbstractPathPoint* newPathPoint) +{ + OPENSIM_THROW(Exception, "Tried to call `replacePathPoint` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain a path). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +void OpenSim::FunctionBasedPath::addPathWrap(WrapObject& aWrapObject) +{ + OPENSIM_THROW(Exception, "Tried to call `addPathWrap` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain wrapping objects). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +void OpenSim::FunctionBasedPath::deletePathWrap(const SimTK::State& s, + int index) +{ + OPENSIM_THROW(Exception, "Tried to call `deletePathWrap` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain wrapping objects). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +void OpenSim::FunctionBasedPath::moveUpPathWrap(const SimTK::State& s, + int index) +{ + OPENSIM_THROW(Exception, "Tried to call `moveUpPathWrap` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain wrapping objects). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +void OpenSim::FunctionBasedPath::moveDownPathWrap(const SimTK::State& s, + int index) +{ + OPENSIM_THROW(Exception, "Tried to call `moveDownPathWrap` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it does not contain wrapping objects). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +void OpenSim::FunctionBasedPath::applyWrapObjects(const SimTK::State& s, + Array& path ) const +{ + OPENSIM_THROW(Exception, "Tried to call `applyWrapObjects` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it doesn't know how to apply wrap objects to a sequence of path points). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} + +void OpenSim::FunctionBasedPath::namePathPoints(int aStartingIndex) +{ + static bool shownOnce = []() { + OpenSim::log_warn("Tried to call `namePathPoints` on a `FunctionBasedPath`. This function call will be ignored (`FunctionBasedPath`s do not contain path points)"); + return true; + }(); + (void)shownOnce; +} + +void OpenSim::FunctionBasedPath::placeNewPathPoint(const SimTK::State& s, + SimTK::Vec3& aOffset, + int index, + const PhysicalFrame& frame) +{ + OPENSIM_THROW(Exception, "Tried to call `placeNwPathPoint` on a `FunctionBasedPath`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it has no path points). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); +} diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h new file mode 100644 index 0000000000..9ca5c92c59 --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -0,0 +1,231 @@ +#ifndef OPENSIM_FUNCTIONBASED_PATH_H_ +#define OPENSIM_FUNCTIONBASED_PATH_H_ + +/* -------------------------------------------------------------------------- * + * OpenSim: FunctionBasedPath.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-2021 TU Delft and the Authors * + * Author(s): Joris Verhagen * + * * + * 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 +#include +#include +#include +#include +#include + +#include + +#ifdef SWIG + #ifdef OSIMSIMULATION_API + #undef OSIMSIMULATION_API + #define OSIMSIMULATION_API + #endif +#endif + +// forward declarations +namespace OpenSim { +class Component; +class Coordinate; +class Model; +class PointBasedPath; +class PointForceDirection; +class PhysicalFrame; +class WrapObject; +class WrapResult; +} + +namespace SimTK { +class State; +} + +namespace OpenSim { +/** + * An `OpenSim::GeometryPath` that computes its length/lengtheningSpeed by + * interpolating a pre-computed Bezier curve. + * + * The precomputed curve can be produced from by fitting an existing + * `OpenSim::PointBasedPath` in a Model. You can use + * `FunctionBasedPath::fromPointBasedPath` to do that in code, or use the + * FunctionBasedPathModelTool do convert all `PointBasedPaths` in an osim into + * `FunctionBasedPath`s. + * + * The curve fitting implementation requires access to the whole OpenSim model + * that the (concrete) `PointBasedPath` is in. This is because the fitting + * implementation permutes the model's coordinates to fit the curve, and + * because a curve's paramaterization depends on its place in the Model as + * a whole. + */ +class OSIMSIMULATION_API FunctionBasedPath : public GeometryPath { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, GeometryPath); + +public: + OpenSim_DECLARE_UNNAMED_PROPERTY(FunctionBasedPathDiscretizationSet, "Discretizations that were used for each OpenSim::Coordinate that the path was fitted against"); + OpenSim_DECLARE_LIST_PROPERTY(Evaluations, double, "The evaluated results of each *permutation* of discretizations. The FunctionBasedPathDiscretizationSet property describes how each OpenSim::Coordinate was discretized. These evaluations are the result of permuting through all possible combinations of discretizations. Effectively, this property contains a N-dimensional 'surface' of points, where each dimension of the surface is a Coordinate, and each dimension of each point is one of the evenly-spaced points in the discretization range [x_begin, x_range] for each dimension"); + + struct Impl; +private: + SimTK::ClonePtr _impl; + +public: + struct FittingParams final { + + // maximum coords that can affect the given PointBasedPath + // + // if this is higher, more paths may be eligible for + // PointBasedPath --> FunctionBasedPath conversion, because some paths may be + // affected by more coordinates than other paths. However, be careful. Increasing + // this also *significantly* increases the memory usage of the function-based fit + // + // must be 0 < v <= 16, or -1 to mean "use a sensible default" + int maxCoordsThatCanAffectPath; + + // number of discretization steps to use for each coordinate during the "probing + // phase" + // + // in the "probing phase", each coordinate is set to this number of evenly-spaced + // values in the range [getRangeMin()..getRangeMax()] (inclusive) to see if changing + // that coordinate has any affect on the path. The higher this value is, the longer + // the probing phase takes, but the higher chance it has of spotting a pertubation + // + // must be >0, or -1 to mean "use a sensible default" + int numProbingDiscretizations; + + // minimum amount that the moment arm of the path must change by during the "probing phase" + // for the coorinate to be classified as affecting the path + // + // must be >0, or <0 to mean "use a sensible default" + double minProbingMomentArmChange; + + // the number of discretization steps for each coordinate that passes the "probing phase" and, + // therefore, is deemed to affect the input (point-based) path + // + // this is effectively "grid granulaity". More discretizations == better fit, but it can increase + // the memory usage of the fit significantly. Assume the path is parameterized as an n-dimensional + // surface. E.g. if you discretize 10 points over 10 dimensions then you may end up with + // 10^10 datapoints (ouch). + // + // must be >0, or -1 to mean "use a sensible default" + int numDiscretizationStepsPerDimension; + + FittingParams(); + }; + + /** + * Tries to compute a `FunctionBasedPath` from the provided `PointBasedPath`, given + * the parameters. + * + * This process can fail (return nullptr) if: + * + * - No coordinates seem to affect the path + * - Too many coordinates (>params.maxCoordsThatCanAffectPath) affect the path + * + * Otherwise, returns a non-nullptr to the fitted path + */ + static std::unique_ptr fromPointBasedPath(const Model& model, + const PointBasedPath& pbp, + FittingParams params); + + FunctionBasedPath(); + FunctionBasedPath(const FunctionBasedPath&); + FunctionBasedPath(FunctionBasedPath&&); + FunctionBasedPath& operator=(const FunctionBasedPath&); + FunctionBasedPath& operator=(FunctionBasedPath&&); + ~FunctionBasedPath() noexcept override; + + void extendFinalizeFromProperties() override; + void extendFinalizeConnections(Component& root) override; + + double getLength(const SimTK::State& s) const override; + void setLength(const SimTK::State& s, double length) const override; + + double getLengtheningSpeed(const SimTK::State& s) const override; + void setLengtheningSpeed(const SimTK::State& s, double speed) const override; + + double computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const override; + + void addInEquivalentForces(const SimTK::State& state, + const double& tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const override; + + void generateDecorations(bool fixed, + const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& appendToThis) const override; + +protected: + void computePath(const SimTK::State& s ) const override; + void computeLengtheningSpeed(const SimTK::State& s) const override; + + double calcLengthAfterPathComputation(const SimTK::State& s, + const Array& currentPath) const override; + + double calcPathLengthChange(const SimTK::State& s, + const WrapObject& wo, + const WrapResult& wr, + const Array& path) const override; + +private: + void getPointForceDirections(const SimTK::State& s, + OpenSim::Array *rPFDs) const override; + + const Array& getCurrentPath(const SimTK::State& s) const override; + + AbstractPathPoint* addPathPoint(const SimTK::State& s, + int index, + const PhysicalFrame& frame) override; + + AbstractPathPoint* appendNewPathPoint(const std::string& proposedName, + const PhysicalFrame& frame, + const SimTK::Vec3& locationOnFrame) override; + + bool canDeletePathPoint(int index) override; + + bool deletePathPoint(const SimTK::State& s, int index) override; + + bool replacePathPoint(const SimTK::State& s, + AbstractPathPoint* oldPathPoint, + AbstractPathPoint* newPathPoint) override; + + + void addPathWrap(WrapObject& aWrapObject) override; + + void deletePathWrap(const SimTK::State& s, int index) override; + + void moveUpPathWrap(const SimTK::State& s, + int index) override; + + void moveDownPathWrap(const SimTK::State& s, + int index) override; + + void applyWrapObjects(const SimTK::State& s, + Array& path) const override; + + void namePathPoints(int aStartingIndex) override; + + void placeNewPathPoint(const SimTK::State& s, + SimTK::Vec3& aOffset, + int index, + const PhysicalFrame& frame) override; +}; +} + +#endif diff --git a/OpenSim/Simulation/Model/FunctionBasedPathDiscretization.h b/OpenSim/Simulation/Model/FunctionBasedPathDiscretization.h new file mode 100644 index 0000000000..4ff2c6c429 --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPathDiscretization.h @@ -0,0 +1,51 @@ +#ifndef FUNCTIONBASEDPATHDISCRETIZATION_H +#define FUNCTIONBASEDPATHDISCRETIZATION_H + +/* -------------------------------------------------------------------------- * + * OpenSim: FunctionBasedPathDiscretization.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-2021 TU Delft and the Authors * + * Author(s): Joris Verhagen * + * * + * 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 +#include + +#include + +namespace OpenSim { + class OSIMSIMULATION_API FunctionBasedPathDiscretization : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathDiscretization, Component); + + public: + OpenSim_DECLARE_PROPERTY(coordinate_abspath, std::string, "The absolute path, in the model, to the OpenSim::Coordinate that this discretization was produced from"); + OpenSim_DECLARE_PROPERTY(x_begin, double, "The lowest OpenSim::Coordinate value that was used for discretization"); + OpenSim_DECLARE_PROPERTY(x_end, double, "The highest OpenSim:::Coordinate value that was used for discretization"); + OpenSim_DECLARE_PROPERTY(num_points, int, "The number of evenly-spaced OpenSim::Coordinate values between [x_begin, x_end] (inclusive) that were used for discretization of the OpenSim::Coordinate. E.g. [x_begin, 1*(x_begin+((x_end-x_begin)/3)), 2*(x_begin+((x_end-x_begin)/3)), x_end]"); + + FunctionBasedPathDiscretization() { + constructProperty_coordinate_abspath(""); + constructProperty_x_begin(0.0); + constructProperty_x_end(0.0); + constructProperty_num_points(0); + } + }; +} + +#endif // FUNCTIONBASEDPATHDISCRETIZATION_H diff --git a/OpenSim/Simulation/Model/FunctionBasedPathDiscretizationSet.h b/OpenSim/Simulation/Model/FunctionBasedPathDiscretizationSet.h new file mode 100644 index 0000000000..9557c03b66 --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPathDiscretizationSet.h @@ -0,0 +1,39 @@ +#ifndef FUNCTIONBASEDPATHDISCRETIZATIONSET_H +#define FUNCTIONBASEDPATHDISCRETIZATIONSET_H + +/* -------------------------------------------------------------------------- * + * OpenSim: FunctionBasedPathDiscretizationSet.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-2021 TU Delft and the Authors * + * Author(s): Joris Verhagen * + * * + * 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 +#include +#include + +namespace OpenSim { + +class OSIMSIMULATION_API FunctionBasedPathDiscretizationSet : public Set { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathDiscretizationSet, Set); +}; + +} + +#endif // FUNCTIONBASEDPATHDISCRETIZATIONSET_H diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 2953703bb7..d612dfc8c5 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -115,75 +115,7 @@ void GeometryPath::extendConnectToModel(Model& aModel) markCacheVariableValid(s, _colorCV); // it is OK at its default value } -//------------------------------------------------------------------------------ -// GENERATE DECORATIONS -//------------------------------------------------------------------------------ -// The GeometryPath takes care of drawing itself here, using information it -// can extract from the supplied state, including position information and -// color information that may have been calculated as late as Stage::Dynamics. -// For example, muscles may want the color to reflect activation level and -// other path-using components might want to use forces (tension). We will -// ensure that the state has been realized to Stage::Dynamics before looking -// at it. (It is only guaranteed to be at Stage::Position here.) -void GeometryPath:: -generateDecorations(bool fixed, const ModelDisplayHints& hints, - const SimTK::State& state, - SimTK::Array_& appendToThis) const -{ - // There is no fixed geometry to generate here. - if (fixed) { return; } - - const Array& pathPoints = getCurrentPath(state); - - assert(pathPoints.size() > 1); - - const AbstractPathPoint* lastPoint = pathPoints[0]; - MobilizedBodyIndex mbix(0); - - Vec3 lastPos = lastPoint->getLocationInGround(state); - if (hints.get_show_path_points()) - DefaultGeometry::drawPathPoint(mbix, lastPos, getColor(state), appendToThis); - Vec3 pos; - - for (int i = 1; i < pathPoints.getSize(); ++i) { - AbstractPathPoint* point = pathPoints[i]; - PathWrapPoint* pwp = dynamic_cast(point); - - if (pwp) { - // A PathWrapPoint provides points on the wrapping surface as Vec3s - Array& surfacePoints = pwp->getWrapPath(); - // The surface points are expressed w.r.t. the wrap surface's body frame. - // Transform the surface points into the ground reference frame to draw - // the surface point as the wrapping portion of the GeometryPath - const Transform& X_BG = pwp->getParentFrame().getTransformInGround(state); - // Cycle through each surface point and draw it the Ground frame - for (int j = 0; jgetLocationInGround(state); - if (hints.get_show_path_points()) - DefaultGeometry::drawPathPoint(mbix, pos, getColor(state), - appendToThis); - // Line segments will be in ground frame - appendToThis.push_back(DecorativeLine(lastPos, pos) - .setLineThickness(4) - .setColor(getColor(state)).setBodyId(0).setIndexOnBody(i)); - lastPos = pos; - } - } -} //_____________________________________________________________________________ /* @@ -200,225 +132,6 @@ void GeometryPath::constructProperties() constructProperty_Appearance(appearance); } -//_____________________________________________________________________________ -/* - * Name the path points based on their position in the set. To keep the - * names up to date, this method should be called every time the path changes. - * - * @param aStartingIndex The index of the first path point to name. - */ -void GeometryPath::namePathPoints(int aStartingIndex) -{ - char indx[5]; - for (int i = aStartingIndex; i < get_PathPointSet().getSize(); i++) - { - sprintf(indx,"%d",i+1); - AbstractPathPoint& point = get_PathPointSet().get(i); - if (point.getName()=="" && hasOwner()) { - point.setName(getOwner().getName() + "-P" + indx); - } - } -} - -//_____________________________________________________________________________ -/* - * get the current path of the path - * - * @return The array of currently active path points. - * - */ -const OpenSim::Array & GeometryPath:: -getCurrentPath(const SimTK::State& s) const -{ - computePath(s); // compute checks if path needs to be recomputed - return getCacheVariableValue< Array >(s, "current_path"); -} - -// get the path as PointForceDirections directions -// CAUTION: the return points are heap allocated; you must delete them yourself! -// (TODO: that is really lame) -void GeometryPath:: -getPointForceDirections(const SimTK::State& s, - OpenSim::Array *rPFDs) const -{ - int i; - AbstractPathPoint* start; - AbstractPathPoint* end; - const OpenSim::PhysicalFrame* startBody; - const OpenSim::PhysicalFrame* endBody; - const Array& currentPath = getCurrentPath(s); - - int np = currentPath.getSize(); - rPFDs->ensureCapacity(np); - - for (i = 0; i < np; i++) { - PointForceDirection *pfd = - new PointForceDirection(currentPath[i]->getLocation(s), - currentPath[i]->getParentFrame(), Vec3(0)); - rPFDs->append(pfd); - } - - for (i = 0; i < np-1; i++) { - start = currentPath[i]; - end = currentPath[i+1]; - startBody = &start->getParentFrame(); - endBody = &end->getParentFrame(); - - if (startBody != endBody) - { - Vec3 posStart, posEnd; - Vec3 direction(0); - - // Find the positions of start and end in the inertial frame. - //engine.getPosition(s, start->getParentFrame(), start->getLocation(), posStart); - posStart = start->getLocationInGround(s); - - //engine.getPosition(s, end->getParentFrame(), end->getLocation(), posEnd); - posEnd = end->getLocationInGround(s); - - // Form a vector from start to end, in the inertial frame. - direction = (posEnd - posStart); - - // Check that the two points are not coincident. - // This can happen due to infeasible wrapping of the path, - // when the origin or insertion enters the wrapping surface. - // This is a temporary fix, since the wrap algorithm should - // return NaN for the points and/or throw an Exception- aseth - if (direction.norm() < SimTK::SignificantReal){ - direction = direction*SimTK::NaN; - } - else{ - direction = direction.normalize(); - } - - // Get resultant direction at each point - rPFDs->get(i)->addToDirection(direction); - rPFDs->get(i+1)->addToDirection(-direction); - } - } -} - -/* add in the equivalent spatial forces on bodies for an applied tension - along the GeometryPath to a set of bodyForces */ -void GeometryPath::addInEquivalentForces(const SimTK::State& s, - const double& tension, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const -{ - AbstractPathPoint* start = NULL; - AbstractPathPoint* end = NULL; - const SimTK::MobilizedBody* bo = NULL; - const SimTK::MobilizedBody* bf = NULL; - const Array& currentPath = getCurrentPath(s); - int np = currentPath.getSize(); - - const SimTK::SimbodyMatterSubsystem& matter = - getModel().getMatterSubsystem(); - - // start point, end point, direction, and force vectors in ground - Vec3 po(0), pf(0), dir(0), force(0); - // partial velocity of point in body expressed in ground - Vec3 dPodq_G(0), dPfdq_G(0); - - // gen force (torque) due to moving point under tension - double fo, ff; - - for (int i = 0; i < np-1; ++i) { - start = currentPath[i]; - end = currentPath[i+1]; - - bo = &start->getParentFrame().getMobilizedBody(); - bf = &end->getParentFrame().getMobilizedBody(); - - if (bo != bf) { - // Find the positions of start and end in the inertial frame. - po = start->getLocationInGround(s); - pf = end->getLocationInGround(s); - - // Form a vector from start to end, in the inertial frame. - dir = (pf - po); - - // Check that the two points are not coincident. - // This can happen due to infeasible wrapping of the path, - // when the origin or insertion enters the wrapping surface. - // This is a temporary fix, since the wrap algorithm should - // return NaN for the points and/or throw an Exception- aseth - if (dir.norm() < SimTK::SignificantReal){ - dir = dir*SimTK::NaN; - } - else{ - dir = dir.normalize(); - } - - force = tension*dir; - - const MovingPathPoint* mppo = - dynamic_cast(start); - - // do the same for the end point of this segment of the path - const MovingPathPoint* mppf = - dynamic_cast(end); - - // add in the tension point forces to body forces - if (mppo) {// moving path point location is a function of the state - // transform of the frame of the point to the base mobilized body - auto X_BF = mppo->getParentFrame().findTransformInBaseFrame(); - bo->applyForceToBodyPoint(s, X_BF*mppo->getLocation(s), force, - bodyForces); - } - else { - // transform of the frame of the point to the base mobilized body - auto X_BF = start->getParentFrame().findTransformInBaseFrame(); - bo->applyForceToBodyPoint(s, X_BF*start->getLocation(s), force, - bodyForces); - } - - if (mppf) {// moving path point location is a function of the state - // transform of the frame of the point to the base mobilized body - auto X_BF = mppf->getParentFrame().findTransformInBaseFrame(); - bf->applyForceToBodyPoint(s, X_BF*mppf->getLocation(s), -force, - bodyForces); - } - else { - // transform of the frame of the point to the base mobilized body - auto X_BF = end->getParentFrame().findTransformInBaseFrame(); - bf->applyForceToBodyPoint(s, X_BF*end->getLocation(s), -force, - bodyForces); - } - - // Now account for the work being done by virtue of the moving - // path point motion relative to the body it is on - if(mppo){ - // torque (genforce) contribution due to relative movement - // of a via point w.r.t. the body it is connected to. - dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s)); - fo = ~dPodq_G*force; - - // get the mobilized body the coordinate is couple to. - const SimTK::MobilizedBody& mpbod = - matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex()); - - // apply the generalized (mobility) force to the coordinate's body - mpbod.applyOneMobilityForce(s, - mppo->getXCoordinate().getMobilizerQIndex(), - fo, mobilityForces); - } - - if(mppf){ - dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s)); - ff = ~dPfdq_G*(-force); - - // get the mobilized body the coordinate is couple to. - const SimTK::MobilizedBody& mpbod = - matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex()); - - mpbod.applyOneMobilityForce(s, - mppf->getXCoordinate().getMobilizerQIndex(), - ff, mobilityForces); - } - } - } -} //_____________________________________________________________________________ /* @@ -434,29 +147,6 @@ void GeometryPath::updateGeometry(const SimTK::State& s) const computePath(s); } -//============================================================================= -// GET -//============================================================================= -//----------------------------------------------------------------------------- -// LENGTH -//----------------------------------------------------------------------------- -//_____________________________________________________________________________ -/* - * Compute the total length of the path. - * - * @return Total length of the path. - */ -double GeometryPath::getLength( const SimTK::State& s) const -{ - computePath(s); // compute checks if path needs to be recomputed - return getCacheVariableValue(s, _lengthCV); -} - -void GeometryPath::setLength( const SimTK::State& s, double length ) const -{ - setCacheVariableValue(s, _lengthCV, length); -} - void GeometryPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const { setCacheVariableValue(s, _colorCV, color); @@ -467,22 +157,6 @@ Vec3 GeometryPath::getColor(const SimTK::State& s) const return getCacheVariableValue(s, _colorCV); } -//_____________________________________________________________________________ -/* - * Compute the lengthening speed of the path. - * - * @return lengthening speed of the path. - */ -double GeometryPath::getLengtheningSpeed( const SimTK::State& s) const -{ - computeLengtheningSpeed(s); - return getCacheVariableValue(s, _speedCV); -} -void GeometryPath::setLengtheningSpeed( const SimTK::State& s, double speed ) const -{ - setCacheVariableValue(s, _speedCV, speed); -} - void GeometryPath::setPreScaleLength( const SimTK::State& s, double length ) { _preScaleLength = length; } @@ -490,300 +164,6 @@ double GeometryPath::getPreScaleLength( const SimTK::State& s) const { return _preScaleLength; } -//============================================================================= -// UTILITY -//============================================================================= -//_____________________________________________________________________________ -/* - * Add a new path point, with default location, to the path. - * - * @param aIndex The position in the pathPointSet to put the new point in. - * @param frame The frame to attach the point to. - * @return Pointer to the newly created path point. - */ -AbstractPathPoint* GeometryPath:: -addPathPoint(const SimTK::State& s, int aIndex, const PhysicalFrame& frame) -{ - PathPoint* newPoint = new PathPoint(); - newPoint->setParentFrame(frame); - Vec3 newLocation(0.0); - // Note: placeNewPathPoint() returns a location by reference. - // It computes a new location according to the index where the new path point - // will be inserted along the path(among the other path points). - placeNewPathPoint(s, newLocation, aIndex, frame); - // Now set computed new location into the newPoint - newPoint->setLocation(newLocation); - upd_PathPointSet().insert(aIndex, newPoint); - - // Rename the path points starting at this new one. - namePathPoints(aIndex); - - // Update start point and end point in the wrap instances so that they - // refer to the same path points they did before the new point - // was added. These indices are 1-based. - aIndex++; - for (int i=0; isetParentFrame(frame); - newPoint->setName(proposedName); - newPoint->setLocation(aPositionOnBody); - upd_PathPointSet().adoptAndAppend(newPoint); - - return newPoint; -} - -//_____________________________________________________________________________ -/* - * Determine an appropriate default XYZ location for a new path point. - * Note that this method is internal and should not be called directly on a new - * point as the point is not really added to the path (done in addPathPoint() - * instead) - * @param aOffset The XYZ location to be determined. - * @param aIndex The position in the pathPointSet to put the new point in. - * @param frame The body to attach the point to. - */ -void GeometryPath:: -placeNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, int aIndex, - const PhysicalFrame& frame) -{ - // The location of the point is determined by moving a 'distance' from 'base' - // along a vector from 'start' to 'end.' 'base' is the existing path point - // that is in or closest to the index aIndex. 'start' and 'end' are existing - // path points--which ones depends on where the new point is being added. - // 'distance' is 0.5 for points added to the middle of a path (so the point - // appears halfway between the two adjacent points), and 0.2 for points that - // are added to either end of the path. If there is only one point in the - // path, the new point is put 0.01 units away in all three dimensions. - if (get_PathPointSet().getSize() > 1) { - int start, end, base; - double distance; - if (aIndex == 0) { - start = 1; - end = 0; - base = end; - distance = 0.2; - } else if (aIndex >= get_PathPointSet().getSize()) { - start = aIndex - 2; - end = aIndex - 1; - base = end; - distance = 0.2; - } else { - start = aIndex; - end = aIndex - 1; - base = start; - distance = 0.5; - } - - const Vec3 startPt = get_PathPointSet()[start].getLocation(s); - const Vec3 endPt = get_PathPointSet()[end].getLocation(s); - const Vec3 basePt = get_PathPointSet()[base].getLocation(s); - - Vec3 startPt2 = get_PathPointSet()[start].getParentFrame() - .findStationLocationInAnotherFrame(s, startPt, frame); - - Vec3 endPt2 = get_PathPointSet()[end].getParentFrame() - .findStationLocationInAnotherFrame(s, endPt, frame); - - aOffset = basePt + distance * (endPt2 - startPt2); - } else if (get_PathPointSet().getSize() == 1) { - aOffset= get_PathPointSet()[0].getLocation(s) + 0.01; - } - else { // first point, do nothing? - } -} - -//_____________________________________________________________________________ -/* - * See if a path point can be deleted. All paths must have at least two - * active path points to define the path. - * - * @param aIndex The index of the point to delete. - * @return Whether or not the point can be deleted. - */ -bool GeometryPath::canDeletePathPoint( int aIndex) -{ - // A path point can be deleted only if there would remain - // at least two other fixed points. - int numOtherFixedPoints = 0; - for (int i = 0; i < get_PathPointSet().getSize(); i++) { - if (i != aIndex) { - if (!( get_PathPointSet().get(i).getConcreteClassName() - ==("ConditionalPathPoint"))) - numOtherFixedPoints++; - } - } - - if (numOtherFixedPoints >= 2) - return true; - - return false; -} - -//_____________________________________________________________________________ -/* - * Delete a path point. - * - * @param aIndex The index of the point to delete. - * @return Whether or not the point was deleted. - */ -bool GeometryPath::deletePathPoint(const SimTK::State& s, int aIndex) -{ - if (canDeletePathPoint(aIndex) == false) - return false; - - upd_PathPointSet().remove(aIndex); - - // rename the path points starting at the deleted position - namePathPoints(aIndex); - - // Update start point and end point in the wrap instances so that they - // refer to the same path points they did before the point was - // deleted. These indices are 1-based. If the point deleted is start - // point or end point, the path wrap range is made smaller by one point. - aIndex++; - for (int i=0; i get_PathPointSet().getSize())) - get_PathWrapSet().get(i).setStartPoint(s, startPoint - 1); - - if ( endPoint > 1 - && aIndex <= endPoint - && ( (endPoint > startPoint) - || (endPoint > get_PathPointSet().getSize()))) - get_PathWrapSet().get(i).setEndPoint(s, endPoint - 1); - } - - return true; -} - -//_____________________________________________________________________________ -/* - * Replace a path point in the set with another point. The new one is made a - * member of all the same groups as the old one, and is inserted in the same - * place the old one occupied. - * - * @param aOldPathPoint Path point to remove. - * @param aNewPathPoint Path point to add. - */ -bool GeometryPath:: -replacePathPoint(const SimTK::State& s, AbstractPathPoint* aOldPathPoint, - AbstractPathPoint* aNewPathPoint) -{ - if (aOldPathPoint != NULL && aNewPathPoint != NULL) { - int count = 0; - int index = get_PathPointSet().getIndex(aOldPathPoint); - // If you're switching from non-via to via, check to make sure that the - // path will be left with at least 2 non-via points. - ConditionalPathPoint* oldVia = - dynamic_cast(aOldPathPoint); - ConditionalPathPoint* newVia = - dynamic_cast(aNewPathPoint); - if (oldVia == NULL && newVia != NULL) { - for (int i=0; i - (&get_PathPointSet().get(i)) == NULL) - count++; - } - } - } else { - count = 2; - } - if (count >= 2 && index >= 0) { - upd_PathPointSet().set(index, aNewPathPoint, true); - //computePath(s); - return true; - } - } - return false; -} - -//_____________________________________________________________________________ -/* - * Create a new wrap instance and add it to the set. - * - * @param aWrapObject The wrap object to use in the new wrap instance. - */ -void GeometryPath::addPathWrap(WrapObject& aWrapObject) -{ - PathWrap* newWrap = new PathWrap(); - newWrap->setWrapObject(aWrapObject); - newWrap->setMethod(PathWrap::hybrid); - upd_PathWrapSet().adoptAndAppend(newWrap); - finalizeFromProperties(); -} - -//_____________________________________________________________________________ -/* - * Move a wrap instance up in the list. Changing the order of wrap instances for - * a path may affect how the path wraps over the wrap objects. - * - * @param aIndex The index of the wrap instance to move up. - */ -void GeometryPath::moveUpPathWrap(const SimTK::State& s, int aIndex) -{ - if (aIndex > 0) { - // Make sure wrap object is not deleted by remove(). - upd_PathWrapSet().setMemoryOwner(false); - - PathWrap& wrap = get_PathWrapSet().get(aIndex); - upd_PathWrapSet().remove(aIndex); - upd_PathWrapSet().insert(aIndex - 1, &wrap); - upd_PathWrapSet().setMemoryOwner(true); - } -} - -//_____________________________________________________________________________ -/* - * Move a wrap instance down in the list. Changing the order of wrap instances - * for a path may affect how the path wraps over the wrap objects. - * - * @param aIndex The index of the wrap instance to move down. - */ -void GeometryPath::moveDownPathWrap(const SimTK::State& s, int aIndex) -{ - if (aIndex < get_PathWrapSet().getSize() - 1) { - // Make sure wrap object is not deleted by remove(). - upd_PathWrapSet().setMemoryOwner(false); - - PathWrap& wrap = get_PathWrapSet().get(aIndex); - upd_PathWrapSet().remove(aIndex); - upd_PathWrapSet().insert(aIndex + 1, &wrap); - upd_PathWrapSet().setMemoryOwner(true); - } -} - -//_____________________________________________________________________________ -/* - * Delete a wrap instance. - * - * @param aIndex The index of the wrap instance to delete. - */ -void GeometryPath::deletePathWrap(const SimTK::State& s, int aIndex) -{ - upd_PathWrapSet().remove(aIndex); - -} - //============================================================================== // SCALING //============================================================================== @@ -801,358 +181,6 @@ extendPostScale(const SimTK::State& s, const ScaleSet& scaleSet) computePath(s); } -//-------------------------------------------------------------------------- -// COMPUTATIONS -//-------------------------------------------------------------------------- -//============================================================================= -// PATH, WRAPPING, AND MOMENT ARM -//============================================================================= -//_____________________________________________________________________________ -/* - * Calculate the current path. - */ -void GeometryPath::computePath(const SimTK::State& s) const -{ - if (isCacheVariableValid(s, _currentPathCV)) { - return; - } - - // Clear the current path. - Array& currentPath = updCacheVariableValue(s, _currentPathCV); - currentPath.setSize(0); - - // Add the active fixed and moving via points to the path. - for (int i = 0; i < get_PathPointSet().getSize(); i++) { - if (get_PathPointSet()[i].isActive(s)) - currentPath.append(&get_PathPointSet()[i]); // <--- !!!!BAD - } - - // Use the current path so far to check for intersection with wrap objects, - // which may add additional points to the path. - applyWrapObjects(s, currentPath); - calcLengthAfterPathComputation(s, currentPath); - - markCacheVariableValid(s, _currentPathCV); -} - -//_____________________________________________________________________________ -/* - * Compute lengthening speed of the path. - */ -void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const -{ - if (isCacheVariableValid(s, _speedCV)) { - return; - } - - const Array& currentPath = getCurrentPath(s); - - double speed = 0.0; - - for (int i = 0; i < currentPath.getSize() - 1; i++) { - speed += currentPath[i]->calcSpeedBetween(s, *currentPath[i+1]); - } - - setLengtheningSpeed(s, speed); -} - -//_____________________________________________________________________________ -/* - * Apply the wrap objects to the current path. - */ -void GeometryPath:: -applyWrapObjects(const SimTK::State& s, Array& path) const -{ - if (get_PathWrapSet().getSize() < 1) - return; - - WrapResult best_wrap; - Array result, order; - - result.setSize(get_PathWrapSet().getSize()); - order.setSize(get_PathWrapSet().getSize()); - - // Set the initial order to be the order they are listed in the path. - for (int i = 0; i < get_PathWrapSet().getSize(); i++) - order[i] = i; - - // If there is only one wrap object, calculate the wrapping only once. - // If there are two or more objects, perform up to 8 iterations where - // the result from one wrap object is used as the starting point for - // the next wrap. - const int maxIterations = get_PathWrapSet().getSize() < 2 ? 1 : 8; - double last_length = SimTK::Infinity; - for (int kk = 0; kk < maxIterations; kk++) - { - for (int i = 0; i < get_PathWrapSet().getSize(); i++) - { - result[i] = 0; - PathWrap& ws = get_PathWrapSet().get(order[i]); - const WrapObject* wo = ws.getWrapObject(); - best_wrap.wrap_pts.setSize(0); - double min_length_change = SimTK::Infinity; - - // First remove this object's wrapping points from the current path. - for (int j = 0; j get_active()) { - // startPoint and endPoint in wrapStruct represent the - // user-defined starting and ending points in the array of path - // points that should be considered for wrapping. These indices - // take into account via points, whether or not they are active. - // Thus they are indices into mp_orig[], not mp[] (also, mp[] - // may contain wrapping points from previous wrap objects, which - // would mess up the starting and ending indices). But the goal - // is to find starting and ending indices in mp[] to consider - // for wrapping over this wrap object. Here is how that is done: - - // 1. startPoint and endPoint are 1-based, so subtract 1 from - // them to get indices into get_PathPointSet(). -1 (or any value - // less than 1) means use the first (or last) point. - const int wrapStart = (ws.getStartPoint() < 1 - ? 0 - : ws.getStartPoint() - 1); - const int wrapEnd = (ws.getEndPoint() < 1 - ? get_PathPointSet().getSize() - 1 - : ws.getEndPoint() - 1); - - // 2. Scan forward from wrapStart in get_PathPointSet() to find - // the first point that is active. Store a pointer to it (smp). - int jfwd = wrapStart; - for (; jfwd <= wrapEnd; jfwd++) - if (get_PathPointSet().get(jfwd).isActive(s)) - break; - if (jfwd > wrapEnd) // there are no active points in the path - return; - const AbstractPathPoint* const smp = &get_PathPointSet().get(jfwd); - - // 3. Scan backwards from wrapEnd in get_PathPointSet() to find - // the last point that is active. Store a pointer to it (emp). - int jrev = wrapEnd; - for (; jrev >= wrapStart; jrev--) - if (get_PathPointSet().get(jrev).isActive(s)) - break; - if (jrev < wrapStart) // there are no active points in the path - return; - const AbstractPathPoint* const emp = &get_PathPointSet().get(jrev); - - // 4. Now find the indices of smp and emp in _currentPath. - int start=-1, end=-1; - for (int j = 0; j < path.getSize(); j++) { - if (path.get(j) == smp) - start = j; - if (path.get(j) == emp) - end = j; - } - if (start == -1 || end == -1) // this should never happen - return; - - // You now have indices into _currentPath (which is a list of - // all currently active points, including wrap points) that - // represent the used-defined range of points to consider for - // wrapping over this wrap object. Check each path segment in - // this range, choosing the best wrap as the one that changes - // the path segment length the least: - for (int pt1 = start; pt1 < end; pt1++) - { - const int pt2 = pt1 + 1; - - // As long as the two points are not auto wrap points on the - // same wrap object, check them for wrapping. - if ( path.get(pt1)->getWrapObject() == NULL - || path.get(pt2)->getWrapObject() == NULL - || ( path.get(pt1)->getWrapObject() - != path.get(pt2)->getWrapObject())) - { - WrapResult wr; - wr.startPoint = pt1; - wr.endPoint = pt2; - - result[i] = wo->wrapPathSegment(s, *path.get(pt1), - *path.get(pt2), ws, wr); - if (result[i] == WrapObject::mandatoryWrap) { - // "mandatoryWrap" means the path actually - // intersected the wrap object. In this case, you - // *must* choose this segment as the "best" one for - // wrapping. If the path has more than one segment - // that intersects the object, the first one is - // taken as the mandatory wrap (this is considered - // an ill-conditioned case). - best_wrap = wr; - // Store the best wrap in the pathWrap for possible - // use next time. - ws.setPreviousWrap(wr); - break; - } else if (result[i] == WrapObject::wrapped) { - // "wrapped" means the path segment was wrapped over - // the object, but you should consider the other - // segments as well to see if one - // wraps with a smaller length change. - double path_length_change = - calcPathLengthChange(s, *wo, wr, path); - if (path_length_change < min_length_change) - { - best_wrap = wr; - // Store the best wrap in the pathWrap for - // possible use next time - ws.setPreviousWrap(wr); - min_length_change = path_length_change; - } else { - // The wrap was not shorter than the current - // minimum, so just free the wrap points that - // were allocated. - wr.wrap_pts.setSize(0); - } - } else { - // Nothing to do. - } - } - } - - // Deallocate previous wrapping points if necessary. - ws.updWrapPoint2().getWrapPath().setSize(0); - - if (best_wrap.wrap_pts.getSize() == 0) { - ws.resetPreviousWrap(); - ws.updWrapPoint2().getWrapPath().setSize(0); - } else { - // If wrapping did occur, copy wrap info into the PathStruct. - ws.updWrapPoint1().getWrapPath().setSize(0); - - Array& wrapPath = ws.updWrapPoint2().getWrapPath(); - wrapPath = best_wrap.wrap_pts; - - // In OpenSim, all conversion to/from the wrap object's - // reference frame will be performed inside - // wrapPathSegment(). Thus, all points in this function will - // be in their respective body reference frames. - // for (j = 0; j < wrapPath.getSize(); j++){ - // convert_from_wrap_object_frame(wo, wrapPath.get(j)); - // convert(ms->modelnum, wrapPath.get(j), wo->segment, - // ms->ground_segment); - // } - - ws.updWrapPoint1().setWrapLength(0.0); - ws.updWrapPoint2().setWrapLength(best_wrap.wrap_path_length); - - ws.updWrapPoint1().setLocation(best_wrap.r1); - ws.updWrapPoint2().setLocation(best_wrap.r2); - - // Now insert the two new wrapping points into mp[] array. - path.insert(best_wrap.endPoint, &ws.updWrapPoint1()); - path.insert(best_wrap.endPoint + 1, &ws.updWrapPoint2()); - } - } - } - - const double length = calcLengthAfterPathComputation(s, path); - if (std::abs(length - last_length) < 0.0005) { - break; - } else { - last_length = length; - } - - if (kk == 0 && get_PathWrapSet().getSize() > 1) { - // If the first wrap was a no wrap, and the second was a no wrap - // because a point was inside the object, switch the order of - // the first two objects and try again. - if ( result[0] == WrapObject::noWrap - && result[1] == WrapObject::insideRadius) - { - order[0] = 1; - order[1] = 0; - - // remove wrap object 0 from the list of path points - PathWrap& ws = get_PathWrapSet().get(0); - for (int j = 0; j < path.getSize(); j++) { - if (path.get(j) == &ws.updWrapPoint1()) { - path.remove(j); // remove the first wrap point - path.remove(j); // remove the second wrap point - break; - } - } - } - } - } -} - -//_____________________________________________________________________________ -/* - * _calc_path_length_change - given the output of a successful path wrap - * over a wrap object, determine the percent change in length of the - * path segment incurred by wrapping. - */ -double GeometryPath:: -calcPathLengthChange(const SimTK::State& s, const WrapObject& wo, - const WrapResult& wr, const Array& path) const -{ - const AbstractPathPoint* pt1 = path.get(wr.startPoint); - const AbstractPathPoint* pt2 = path.get(wr.endPoint); - - double straight_length = pt1->calcDistanceBetween(s, *pt2); - - double wrap_length = pt1->calcDistanceBetween(s, wo.getFrame(), wr.r1); - wrap_length += wr.wrap_path_length; - wrap_length += pt2->calcDistanceBetween(s, wo.getFrame(), wr.r2); - - return wrap_length - straight_length; // return absolute diff, not relative -} - -//_____________________________________________________________________________ -/* - * Compute the total length of the path. This function - * assumes that the path has already been updated. - */ -double GeometryPath:: -calcLengthAfterPathComputation(const SimTK::State& s, - const Array& currentPath) const -{ - double length = 0.0; - - for (int i = 0; i < currentPath.getSize() - 1; i++) { - const AbstractPathPoint* p1 = currentPath[i]; - const AbstractPathPoint* p2 = currentPath[i+1]; - - // If both points are wrap points on the same wrap object, then this - // path segment wraps over the surface of a wrap object, so just add in - // the pre-calculated length. - if ( p1->getWrapObject() - && p2->getWrapObject() - && p1->getWrapObject() == p2->getWrapObject()) - { - const PathWrapPoint* smwp = dynamic_cast(p2); - if (smwp) - length += smwp->getWrapLength(); - } else { - length += p1->calcDistanceBetween(s, *p2); - } - } - - setLength(s,length); - return( length ); -} - -//_____________________________________________________________________________ -/* - * Compute the path's moment arms for specified coordinate. - * - * @param aCoord, the coordinate - */ -double GeometryPath:: -computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const -{ - if (!_maSolver) - const_cast(this)->_maSolver.reset(new MomentArmSolver(*_model)); - - return _maSolver->solve(s, aCoord, *this); -} - //_____________________________________________________________________________ // Override default implementation by object to intercept and fix the XML node // underneath the model to match current version. diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index 045efe5888..6b50fb93d0 100644 --- a/OpenSim/Simulation/Model/GeometryPath.h +++ b/OpenSim/Simulation/Model/GeometryPath.h @@ -1,5 +1,6 @@ #ifndef OPENSIM_GEOMETRY_PATH_H_ #define OPENSIM_GEOMETRY_PATH_H_ + /* -------------------------------------------------------------------------- * * OpenSim: GeometryPath.h * * -------------------------------------------------------------------------- * @@ -23,14 +24,12 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ - // INCLUDE #include -#include "OpenSim/Simulation/Model/ModelComponent.h" -#include "PathPointSet.h" -#include #include - +#include +#include +#include #ifdef SWIG #ifdef OSIMSIMULATION_API @@ -47,23 +46,35 @@ class ScaleSet; class WrapResult; class WrapObject; -//============================================================================= -//============================================================================= /** - * A base class representing a path (muscle, ligament, etc.). - * - * @author Peter Loan - * @version 1.0 - */ + * A base class that represents a path that has a computable length and + * lengthening speed. + * + * This class is typically used in places where the model needs to simulate + * the changes in a path over time. For example, in `OpenSim::Muscle`s, + * `OpenSim::Ligament`s, etc. + * + * This class *only* defines a length and lenghtning speed. Do not assume that + * an `OpenSim::GeometryPath` is a straight line between two points, or assume + * that it is many straight lines between `n` points. The derived + * implementation may define a path using points, or it may define a path using + * a curve fit. It may also define a path as + * `length == 17.3f && lengtheningSpeed == 5.0f`. All of those definitions are + * *logically* valid - even if they aren't *functionally* valid + */ class OSIMSIMULATION_API GeometryPath : public ModelComponent { -OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); - //============================================================================= - // OUTPUTS - //============================================================================= +OpenSim_DECLARE_ABSTRACT_OBJECT(GeometryPath, ModelComponent); + +//============================================================================= +// OUTPUTS +//============================================================================= OpenSim_DECLARE_OUTPUT(length, double, getLength, SimTK::Stage::Position); - // - OpenSim_DECLARE_OUTPUT(lengthening_speed, double, getLengtheningSpeed, - SimTK::Stage::Velocity); + + OpenSim_DECLARE_OUTPUT( + lengthening_speed, + double, + getLengtheningSpeed, + SimTK::Stage::Velocity); //============================================================================= // DATA @@ -72,13 +83,15 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); OpenSim_DECLARE_UNNAMED_PROPERTY(Appearance, "Default appearance attributes for this GeometryPath"); -private: +// from private to public because now required in PBP +public: OpenSim_DECLARE_UNNAMED_PROPERTY(PathPointSet, "The set of points defining the path"); OpenSim_DECLARE_UNNAMED_PROPERTY(PathWrapSet, "The wrap objects that are associated with this path"); +protected: // used for scaling tendon and fiber lengths double _preScaleLength; @@ -95,46 +108,19 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); //============================================================================= // METHODS //============================================================================= - //-------------------------------------------------------------------------- - // CONSTRUCTION - //-------------------------------------------------------------------------- public: GeometryPath(); - ~GeometryPath() override = default; - - const PathPointSet& getPathPointSet() const { return get_PathPointSet(); } - PathPointSet& updPathPointSet() { return upd_PathPointSet(); } - const PathWrapSet& getWrapSet() const { return get_PathWrapSet(); } - PathWrapSet& updWrapSet() { return upd_PathWrapSet(); } - void addPathWrap(WrapObject& aWrapObject); - //-------------------------------------------------------------------------- // UTILITY //-------------------------------------------------------------------------- - AbstractPathPoint* addPathPoint(const SimTK::State& s, int index, - const PhysicalFrame& frame); - AbstractPathPoint* appendNewPathPoint(const std::string& proposedName, - const PhysicalFrame& frame, const SimTK::Vec3& locationOnFrame); - bool canDeletePathPoint( int index); - bool deletePathPoint(const SimTK::State& s, int index); - - void moveUpPathWrap(const SimTK::State& s, int index); - void moveDownPathWrap(const SimTK::State& s, int index); - void deletePathWrap(const SimTK::State& s, int index); - bool replacePathPoint(const SimTK::State& s, AbstractPathPoint* oldPathPoint, - AbstractPathPoint* newPathPoint); - - //-------------------------------------------------------------------------- - // GET - //-------------------------------------------------------------------------- - /** If you call this prior to extendAddToSystem() it will be used to initialize the color cache variable. Otherwise %GeometryPath will choose its own default which varies depending on owner. **/ void setDefaultColor(const SimTK::Vec3& color) { updProperty_Appearance().setValueIsDefault(false); upd_Appearance().set_color(color); - }; + } + /** Returns the color that will be used to initialize the color cache at the next extendAddToSystem() call. The actual color used to draw the path will be taken from the cache variable, so may have changed. **/ @@ -155,19 +141,20 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); @see setDefaultColor() **/ SimTK::Vec3 getColor(const SimTK::State& s) const; - double getLength( const SimTK::State& s) const; - void setLength( const SimTK::State& s, double length) const; double getPreScaleLength( const SimTK::State& s) const; void setPreScaleLength( const SimTK::State& s, double preScaleLength); - const Array& getCurrentPath( const SimTK::State& s) const; - double getLengtheningSpeed(const SimTK::State& s) const; - void setLengtheningSpeed( const SimTK::State& s, double speed ) const; + /** Get methods are made non-const as when the GeometryPath is a FunctionBased- + Path, the Interpolation object inside of it changes after every evaluation **/ + virtual double getLength( const SimTK::State& s) const = 0; + virtual void setLength( const SimTK::State& s, double length) const = 0; + virtual double getLengtheningSpeed(const SimTK::State& s) const = 0; + virtual void setLengtheningSpeed( const SimTK::State& s, double speed ) const = 0; /** get the path as PointForceDirections directions, which can be used to apply tension to bodies the points are connected to.*/ - void getPointForceDirections(const SimTK::State& s, - OpenSim::Array *rPFDs) const; + virtual void getPointForceDirections(const SimTK::State& s, + OpenSim::Array *rPFDs) const = 0; /** add in the equivalent body and generalized forces to be applied to the multibody system resulting from a tension along the GeometryPath @@ -176,16 +163,17 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); @param[in,out] bodyForces Vector of SpatialVec's (torque, force) on bodies @param[in,out] mobilityForces Vector of generalized forces, one per mobility */ - void addInEquivalentForces(const SimTK::State& state, - const double& tension, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const; + virtual void addInEquivalentForces(const SimTK::State& state, + const double& tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const = 0; //-------------------------------------------------------------------------- // COMPUTATIONS //-------------------------------------------------------------------------- - virtual double computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const; + virtual double computeMomentArm(const SimTK::State& s, + const Coordinate& aCoord) const = 0; //-------------------------------------------------------------------------- // SCALING @@ -213,36 +201,71 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); void extendInitStateFromProperties(SimTK::State& s) const override; void extendAddToSystem(SimTK::MultibodySystem& system) const override; - // Visual support GeometryPath drawing in SimTK visualizer. - void generateDecorations( - bool fixed, - const ModelDisplayHints& hints, - const SimTK::State& state, - SimTK::Array_& appendToThis) const - override; - void extendFinalizeFromProperties() override; -private: +protected: + virtual void computePath(const SimTK::State& s ) const = 0; + + virtual double calcLengthAfterPathComputation( + const SimTK::State& s, + const Array& currentPath) const = 0; - void computePath(const SimTK::State& s ) const; - void computeLengtheningSpeed(const SimTK::State& s) const; - void applyWrapObjects(const SimTK::State& s, Array& path ) const; - double calcPathLengthChange(const SimTK::State& s, const WrapObject& wo, - const WrapResult& wr, - const Array& path) const; - double calcLengthAfterPathComputation - (const SimTK::State& s, const Array& currentPath) const; + virtual void generateDecorations( + bool fixed, + const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& appendToThis) const override = 0; void constructProperties(); - void namePathPoints(int aStartingIndex); - void placeNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, - int index, const PhysicalFrame& frame); //-------------------------------------------------------------------------- // Implement Object interface. //-------------------------------------------------------------------------- /** Override of the default implementation to account for versioning. */ - void updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber = -1) override; + void updateFromXMLNode( + SimTK::Xml::Element& aNode, + int versionNumber = -1) override; + + + + +// related to pathpoints, so has to be removed in release +protected: + virtual double calcPathLengthChange(const SimTK::State& s, const WrapObject& wo, + const WrapResult& wr, + const Array& path) const = 0; + virtual void computeLengtheningSpeed(const SimTK::State& s) const = 0; + +public: + const PathPointSet& getPathPointSet() const { return get_PathPointSet(); } + void setPathPointSet(const PathPointSet& pps) { upd_PathPointSet() = pps; } + PathPointSet& updPathPointSet() { return upd_PathPointSet(); } + + const PathWrapSet& getWrapSet() const { return get_PathWrapSet(); } + void setPathWrapSet(const PathWrapSet& pws) { upd_PathWrapSet() = pws; } + PathWrapSet& updWrapSet() { return upd_PathWrapSet(); } + virtual void addPathWrap(WrapObject& aWrapObject) = 0; + +public: + virtual const Array& getCurrentPath( const SimTK::State& s) const = 0; + virtual AbstractPathPoint* addPathPoint(const SimTK::State& s, int index, + const PhysicalFrame& frame) = 0; + virtual AbstractPathPoint* appendNewPathPoint(const std::string& proposedName, + const PhysicalFrame& frame, const SimTK::Vec3& locationOnFrame) = 0; + virtual bool canDeletePathPoint(int index) = 0; + virtual bool deletePathPoint(const SimTK::State& s, int index) = 0; + virtual bool replacePathPoint(const SimTK::State& s, + AbstractPathPoint* oldPathPoint, + AbstractPathPoint* newPathPoint) = 0; + + virtual void moveUpPathWrap(const SimTK::State& s, int index) = 0; + virtual void moveDownPathWrap(const SimTK::State& s, int index) = 0; + virtual void deletePathWrap(const SimTK::State& s, int index) = 0; + + virtual void applyWrapObjects(const SimTK::State& s, Array& path ) const = 0; + + virtual void namePathPoints(int aStartingIndex) = 0; + virtual void placeNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, + int index, const PhysicalFrame& frame) = 0; //============================================================================= }; // END of class GeometryPath diff --git a/OpenSim/Simulation/Model/Ligament.cpp b/OpenSim/Simulation/Model/Ligament.cpp index 942a90e426..37eb830cc2 100644 --- a/OpenSim/Simulation/Model/Ligament.cpp +++ b/OpenSim/Simulation/Model/Ligament.cpp @@ -25,7 +25,9 @@ // INCLUDES //============================================================================= #include "Ligament.h" -#include "GeometryPath.h" +//#include "GeometryPath.h" +#include "PointBasedPath.h" +#include "FunctionBasedPath.h" #include "PointForceDirection.h" #include @@ -63,7 +65,7 @@ Ligament::Ligament() void Ligament::constructProperties() { setAuthors("Peter Loan"); - constructProperty_GeometryPath(GeometryPath()); +// constructProperty_GeometryPath(GeometryPath()); constructProperty_resting_length(0.0); constructProperty_pcsa_force(0.0); @@ -244,7 +246,8 @@ void Ligament::computeForce(const SimTK::State& s, setCacheVariableValue(s, _tensionCV, force); OpenSim::Array PFDs; - path.getPointForceDirections(s, &PFDs); +// path.getPointForceDirections(s, &PFDs); + path.addInEquivalentForces(s,force,bodyForces,generalizedForces); for (int i=0; i < PFDs.getSize(); i++) { applyForceToPoint(s, PFDs[i]->frame(), PFDs[i]->point(), diff --git a/OpenSim/Simulation/Model/ModelComponent.h b/OpenSim/Simulation/Model/ModelComponent.h index f892c5eb38..7741dc9f8b 100644 --- a/OpenSim/Simulation/Model/ModelComponent.h +++ b/OpenSim/Simulation/Model/ModelComponent.h @@ -264,7 +264,7 @@ template friend class ModelComponentSet; * ModelComponent interface. ModelComponent::extendFinalizeConnections() * ensures that extendConnectToModel() on ModelComponent subcomponents are * invoked. **/ - void extendFinalizeConnections(Component& root) override final; + void extendFinalizeConnections(Component& root) override; const SimTK::DefaultSystemSubsystem& getDefaultSubsystem() const; const SimTK::DefaultSystemSubsystem& updDefaultSubsystem(); diff --git a/OpenSim/Simulation/Model/Muscle.cpp b/OpenSim/Simulation/Model/Muscle.cpp index 3d1544109d..9747f81b51 100644 --- a/OpenSim/Simulation/Model/Muscle.cpp +++ b/OpenSim/Simulation/Model/Muscle.cpp @@ -26,7 +26,9 @@ //============================================================================= #include "Muscle.h" -#include "GeometryPath.h" +//#include "GeometryPath.h" +#include "PointBasedPath.h" +#include "FunctionBasedPath.h" #include "Model.h" #include diff --git a/OpenSim/Simulation/Model/PathActuator.cpp b/OpenSim/Simulation/Model/PathActuator.cpp index 130965241c..dee681ca29 100644 --- a/OpenSim/Simulation/Model/PathActuator.cpp +++ b/OpenSim/Simulation/Model/PathActuator.cpp @@ -62,7 +62,7 @@ void PathActuator::setNull() */ void PathActuator::constructProperties() { - constructProperty_GeometryPath(GeometryPath()); + constructProperty_GeometryPath(PointBasedPath()); constructProperty_optimal_force(1.0); } @@ -107,6 +107,7 @@ double PathActuator::getOptimalForce() const */ double PathActuator::getLength(const SimTK::State& s) const { +// return path->getLength(s); return getGeometryPath().getLength(s); } //_____________________________________________________________________________ @@ -117,6 +118,7 @@ double PathActuator::getLength(const SimTK::State& s) const */ double PathActuator::getLengtheningSpeed(const SimTK::State& s) const { +// return path->getLengtheningSpeed(s); return getGeometryPath().getLengtheningSpeed(s); } //_____________________________________________________________________________ @@ -176,7 +178,8 @@ void PathActuator::computeForce( const SimTK::State& s, const GeometryPath &path = getGeometryPath(); // compute path's lengthening speed if necessary - double speed = path.getLengtheningSpeed(s); + double speed = getGeometryPath().getLengtheningSpeed(s); +// double speed = path.getLengtheningSpeed(s); // the lengthening speed of this actuator is the "speed" of the actuator // used to compute power diff --git a/OpenSim/Simulation/Model/PathActuator.h b/OpenSim/Simulation/Model/PathActuator.h index 49e1f5b2ec..b489c9bd0a 100644 --- a/OpenSim/Simulation/Model/PathActuator.h +++ b/OpenSim/Simulation/Model/PathActuator.h @@ -24,7 +24,9 @@ * -------------------------------------------------------------------------- */ #include "Actuator.h" -#include "GeometryPath.h" +//#include "GeometryPath.h" +#include "PointBasedPath.h" +#include "FunctionBasedPath.h" //============================================================================= //============================================================================= @@ -50,6 +52,8 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator { //============================================================================= OpenSim_DECLARE_UNNAMED_PROPERTY(GeometryPath, "The set of points defining the path of the actuator."); + OpenSim_DECLARE_UNNAMED_PROPERTY(PointBasedPath, + "The set of points defining the path of the actuator but on a lower abstraction."); OpenSim_DECLARE_PROPERTY(optimal_force, double, "The maximum force this actuator can produce."); @@ -69,9 +73,13 @@ class OSIMSIMULATION_API PathActuator : public ScalarActuator { // Path GeometryPath& updGeometryPath() { return upd_GeometryPath(); } const GeometryPath& getGeometryPath() const - { return get_GeometryPath(); } + { return get_GeometryPath(); } bool hasGeometryPath() const override { return true;}; + PointBasedPath& updPointBasedPath() { return upd_PointBasedPath(); } + const PointBasedPath& getPointBasedPath() const + { return get_PointBasedPath();} + // OPTIMAL FORCE void setOptimalForce(double aOptimalForce); double getOptimalForce() const override; diff --git a/OpenSim/Simulation/Model/PathSpring.cpp b/OpenSim/Simulation/Model/PathSpring.cpp index 2ba09fac02..35bc2cc42a 100644 --- a/OpenSim/Simulation/Model/PathSpring.cpp +++ b/OpenSim/Simulation/Model/PathSpring.cpp @@ -25,7 +25,9 @@ // INCLUDES //============================================================================= #include "PathSpring.h" -#include "GeometryPath.h" +//#include "GeometryPath.h" +#include "PointBasedPath.h" +#include "FunctionBasedPath.h" #include "PointForceDirection.h" //============================================================================= @@ -64,7 +66,7 @@ PathSpring::PathSpring(const string& name, double restLength, void PathSpring::constructProperties() { setAuthors("Ajay Seth"); - constructProperty_GeometryPath(GeometryPath()); +// constructProperty_GeometryPath(GeometryPath()); constructProperty_resting_length(SimTK::NaN); constructProperty_stiffness(SimTK::NaN); constructProperty_dissipation(SimTK::NaN); @@ -206,7 +208,8 @@ void PathSpring::computeForce(const SimTK::State& s, const double& tension = getTension(s); OpenSim::Array PFDs; - path.getPointForceDirections(s, &PFDs); +// path.getPointForceDirections(s, &PFDs); + path.addInEquivalentForces(s,tension,bodyForces,generalizedForces); for (int i=0; i < PFDs.getSize(); i++) { applyForceToPoint(s, PFDs[i]->frame(), PFDs[i]->point(), diff --git a/OpenSim/Simulation/Model/PointBasedPath.cpp b/OpenSim/Simulation/Model/PointBasedPath.cpp new file mode 100644 index 0000000000..6da8622162 --- /dev/null +++ b/OpenSim/Simulation/Model/PointBasedPath.cpp @@ -0,0 +1,992 @@ +#include "PointBasedPath.h" +#include "GeometryPath.h" +#include "ConditionalPathPoint.h" +#include "MovingPathPoint.h" +#include "PointForceDirection.h" +#include "Model.h" +#include + +using namespace std; +using namespace OpenSim; +using namespace SimTK; +using SimTK::Vec3; + +// Default onstructor +PointBasedPath::PointBasedPath() : GeometryPath(){ + +} + +double PointBasedPath::getLength( const SimTK::State& s) const +{ + computePath(s); + return getCacheVariableValue(s, _lengthCV); +} + +void PointBasedPath::setLength(const SimTK::State &s, double length) const +{ + setCacheVariableValue(s, _lengthCV, length); +} + + +double PointBasedPath::getLengtheningSpeed( const SimTK::State& s) const +{ + computeLengtheningSpeed(s); + return getCacheVariableValue(s, _speedCV); +} + +void PointBasedPath::setLengtheningSpeed(const SimTK::State &s, double speed) const +{ + setCacheVariableValue(s, _speedCV, speed); +} + +double PointBasedPath::computeMomentArm(const SimTK::State& s, + const Coordinate& aCoord) const +{ + if (!_maSolver) + const_cast(this)->_maSolver.reset(new MomentArmSolver(*_model)); + + return _maSolver->solve(s, aCoord, *this); +} + +//////////////////////////////// +// Directly from GeometryPath // +//////////////////////////////// +/* add in the equivalent spatial forces on bodies for an applied tension + along the GeometryPath to a set of bodyForces */ +void PointBasedPath::addInEquivalentForces(const SimTK::State& s, + const double& tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const +{ + AbstractPathPoint* start = NULL; + AbstractPathPoint* end = NULL; + const SimTK::MobilizedBody* bo = NULL; + const SimTK::MobilizedBody* bf = NULL; + const Array& currentPath = getCurrentPath(s); + int np = currentPath.getSize(); + + const SimTK::SimbodyMatterSubsystem& matter = + getModel().getMatterSubsystem(); + + // start point, end point, direction, and force vectors in ground + Vec3 po(0), pf(0), dir(0), force(0); + // partial velocity of point in body expressed in ground + Vec3 dPodq_G(0), dPfdq_G(0); + + // gen force (torque) due to moving point under tension + double fo, ff; + + for (int i = 0; i < np-1; ++i) { + start = currentPath[i]; + end = currentPath[i+1]; + + bo = &start->getParentFrame().getMobilizedBody(); + bf = &end->getParentFrame().getMobilizedBody(); + + if (bo != bf) { + // Find the positions of start and end in the inertial frame. + po = start->getLocationInGround(s); + pf = end->getLocationInGround(s); + + // Form a vector from start to end, in the inertial frame. + dir = (pf - po); + + // Check that the two points are not coincident. + // This can happen due to infeasible wrapping of the path, + // when the origin or insertion enters the wrapping surface. + // This is a temporary fix, since the wrap algorithm should + // return NaN for the points and/or throw an Exception- aseth + if (dir.norm() < SimTK::SignificantReal){ + dir = dir*SimTK::NaN; + } + else{ + dir = dir.normalize(); + } + + force = tension*dir; + + const MovingPathPoint* mppo = + dynamic_cast(start); + + // do the same for the end point of this segment of the path + const MovingPathPoint* mppf = + dynamic_cast(end); + + // add in the tension point forces to body forces + if (mppo) {// moving path point location is a function of the state + // transform of the frame of the point to the base mobilized body + auto X_BF = mppo->getParentFrame().findTransformInBaseFrame(); + bo->applyForceToBodyPoint(s, X_BF*mppo->getLocation(s), force, + bodyForces); + } + else { + // transform of the frame of the point to the base mobilized body + auto X_BF = start->getParentFrame().findTransformInBaseFrame(); + bo->applyForceToBodyPoint(s, X_BF*start->getLocation(s), force, + bodyForces); + } + + if (mppf) {// moving path point location is a function of the state + // transform of the frame of the point to the base mobilized body + auto X_BF = mppf->getParentFrame().findTransformInBaseFrame(); + bf->applyForceToBodyPoint(s, X_BF*mppf->getLocation(s), -force, + bodyForces); + } + else { + // transform of the frame of the point to the base mobilized body + auto X_BF = end->getParentFrame().findTransformInBaseFrame(); + bf->applyForceToBodyPoint(s, X_BF*end->getLocation(s), -force, + bodyForces); + } + + // Now account for the work being done by virtue of the moving + // path point motion relative to the body it is on + if(mppo){ + // torque (genforce) contribution due to relative movement + // of a via point w.r.t. the body it is connected to. + dPodq_G = bo->expressVectorInGroundFrame(s, start->getdPointdQ(s)); + fo = ~dPodq_G*force; + + // get the mobilized body the coordinate is couple to. + const SimTK::MobilizedBody& mpbod = + matter.getMobilizedBody(mppo->getXCoordinate().getBodyIndex()); + + // apply the generalized (mobility) force to the coordinate's body + mpbod.applyOneMobilityForce(s, + mppo->getXCoordinate().getMobilizerQIndex(), + fo, mobilityForces); + } + + if(mppf){ + dPfdq_G = bf->expressVectorInGroundFrame(s, end->getdPointdQ(s)); + ff = ~dPfdq_G*(-force); + + // get the mobilized body the coordinate is couple to. + const SimTK::MobilizedBody& mpbod = + matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex()); + + mpbod.applyOneMobilityForce(s, + mppf->getXCoordinate().getMobilizerQIndex(), + ff, mobilityForces); + } + } + } +} + +// get the path as PointForceDirections directions +// CAUTION: the return points are heap allocated; you must delete them yourself! +// (TODO: that is really lame) +void PointBasedPath:: +getPointForceDirections(const SimTK::State& s, + OpenSim::Array *rPFDs) const +{ + int i; + AbstractPathPoint* start; + AbstractPathPoint* end; + const OpenSim::PhysicalFrame* startBody; + const OpenSim::PhysicalFrame* endBody; + const Array& currentPath = getCurrentPath(s); + + int np = currentPath.getSize(); + rPFDs->ensureCapacity(np); + + for (i = 0; i < np; i++) { + PointForceDirection *pfd = + new PointForceDirection(currentPath[i]->getLocation(s), + currentPath[i]->getParentFrame(), Vec3(0)); + rPFDs->append(pfd); + } + + for (i = 0; i < np-1; i++) { + start = currentPath[i]; + end = currentPath[i+1]; + startBody = &start->getParentFrame(); + endBody = &end->getParentFrame(); + + if (startBody != endBody) + { + Vec3 posStart, posEnd; + Vec3 direction(0); + + // Find the positions of start and end in the inertial frame. + //engine.getPosition(s, start->getParentFrame(), start->getLocation(), posStart); + posStart = start->getLocationInGround(s); + + //engine.getPosition(s, end->getParentFrame(), end->getLocation(), posEnd); + posEnd = end->getLocationInGround(s); + + // Form a vector from start to end, in the inertial frame. + direction = (posEnd - posStart); + + // Check that the two points are not coincident. + // This can happen due to infeasible wrapping of the path, + // when the origin or insertion enters the wrapping surface. + // This is a temporary fix, since the wrap algorithm should + // return NaN for the points and/or throw an Exception- aseth + if (direction.norm() < SimTK::SignificantReal){ + direction = direction*SimTK::NaN; + } + else{ + direction = direction.normalize(); + } + + // Get resultant direction at each point + rPFDs->get(i)->addToDirection(direction); + rPFDs->get(i+1)->addToDirection(-direction); + } + } +} + +//_____________________________________________________________________________ +/* + * Calculate the current path. + */ +void PointBasedPath::computePath(const SimTK::State& s) const +{ + if (isCacheVariableValid(s, _currentPathCV)) { + return; + } + + // Clear the current path. + Array& currentPath = updCacheVariableValue(s, _currentPathCV); + currentPath.setSize(0); + + // Add the active fixed and moving via points to the path. + for (int i = 0; i < get_PathPointSet().getSize(); i++) { + if (get_PathPointSet()[i].isActive(s)) + currentPath.append(&get_PathPointSet()[i]); // <--- !!!!BAD + } + + // Use the current path so far to check for intersection with wrap objects, + // which may add additional points to the path. + applyWrapObjects(s, currentPath); + calcLengthAfterPathComputation(s, currentPath); + + markCacheVariableValid(s, _currentPathCV); +} + +//_____________________________________________________________________________ +/* + * Compute lengthening speed of the path. + */ +void PointBasedPath::computeLengtheningSpeed(const SimTK::State& s) const +{ + if (isCacheVariableValid(s, _speedCV)) { + return; + } + + const Array& currentPath = getCurrentPath(s); + + double speed = 0.0; + + for (int i = 0; i < currentPath.getSize() - 1; i++) { + speed += currentPath[i]->calcSpeedBetween(s, *currentPath[i+1]); + } + + setLengtheningSpeed(s, speed); +} + +//_____________________________________________________________________________ +/* + * _calc_path_length_change - given the output of a successful path wrap + * over a wrap object, determine the percent change in length of the + * path segment incurred by wrapping. + */ +double PointBasedPath:: +calcPathLengthChange(const SimTK::State& s, const WrapObject& wo, + const WrapResult& wr, const Array& path) const +{ + const AbstractPathPoint* pt1 = path.get(wr.startPoint); + const AbstractPathPoint* pt2 = path.get(wr.endPoint); + + double straight_length = pt1->calcDistanceBetween(s, *pt2); + + double wrap_length = pt1->calcDistanceBetween(s, wo.getFrame(), wr.r1); + wrap_length += wr.wrap_path_length; + wrap_length += pt2->calcDistanceBetween(s, wo.getFrame(), wr.r2); + + return wrap_length - straight_length; // return absolute diff, not relative +} + +//_____________________________________________________________________________ +/* + * Compute the total length of the path. This function + * assumes that the path has already been updated. + */ +double PointBasedPath:: +calcLengthAfterPathComputation(const SimTK::State& s, + const Array& currentPath) const +{ + double length = 0.0; + + for (int i = 0; i < currentPath.getSize() - 1; i++) { + const AbstractPathPoint* p1 = currentPath[i]; + const AbstractPathPoint* p2 = currentPath[i+1]; + + // If both points are wrap points on the same wrap object, then this + // path segment wraps over the surface of a wrap object, so just add in + // the pre-calculated length. + if ( p1->getWrapObject() + && p2->getWrapObject() + && p1->getWrapObject() == p2->getWrapObject()) + { + const PathWrapPoint* smwp = dynamic_cast(p2); + if (smwp) + length += smwp->getWrapLength(); + } else { + length += p1->calcDistanceBetween(s, *p2); + } + } + + setLength(s,length); + return( length ); +} + +//------------------------------------------------------------------------------ +// GENERATE DECORATIONS +//------------------------------------------------------------------------------ +// The GeometryPath takes care of drawing itself here, using information it +// can extract from the supplied state, including position information and +// color information that may have been calculated as late as Stage::Dynamics. +// For example, muscles may want the color to reflect activation level and +// other path-using components might want to use forces (tension). We will +// ensure that the state has been realized to Stage::Dynamics before looking +// at it. (It is only guaranteed to be at Stage::Position here.) +void PointBasedPath:: +generateDecorations(bool fixed, const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& appendToThis) const +{ + // There is no fixed geometry to generate here. + if (fixed) { return; } + + const Array& pathPoints = getCurrentPath(state); + + assert(pathPoints.size() > 1); + + const AbstractPathPoint* lastPoint = pathPoints[0]; + MobilizedBodyIndex mbix(0); + + Vec3 lastPos = lastPoint->getLocationInGround(state); + if (hints.get_show_path_points()) + DefaultGeometry::drawPathPoint(mbix, lastPos, getColor(state), appendToThis); + + Vec3 pos; + + for (int i = 1; i < pathPoints.getSize(); ++i) { + AbstractPathPoint* point = pathPoints[i]; + PathWrapPoint* pwp = dynamic_cast(point); + + if (pwp) { + // A PathWrapPoint provides points on the wrapping surface as Vec3s + Array& surfacePoints = pwp->getWrapPath(); + // The surface points are expressed w.r.t. the wrap surface's body frame. + // Transform the surface points into the ground reference frame to draw + // the surface point as the wrapping portion of the GeometryPath + const Transform& X_BG = pwp->getParentFrame().getTransformInGround(state); + // Cycle through each surface point and draw it the Ground frame + for (int j = 0; jgetLocationInGround(state); + if (hints.get_show_path_points()) + DefaultGeometry::drawPathPoint(mbix, pos, getColor(state), + appendToThis); + // Line segments will be in ground frame + appendToThis.push_back(DecorativeLine(lastPos, pos) + .setLineThickness(4) + .setColor(getColor(state)).setBodyId(0).setIndexOnBody(i)); + lastPos = pos; + } + } +} + + + + + + + + + + + + + + + + + + + + + + + + + + + + +//_____________________________________________________________________________ +/* + * Create a new wrap instance and add it to the set. + * + * @param aWrapObject The wrap object to use in the new wrap instance. + */ +void PointBasedPath::addPathWrap(WrapObject& aWrapObject) +{ + PathWrap* newWrap = new PathWrap(); + newWrap->setWrapObject(aWrapObject); + newWrap->setMethod(PathWrap::hybrid); + upd_PathWrapSet().adoptAndAppend(newWrap); + finalizeFromProperties(); +} + +//_____________________________________________________________________________ +/* + * get the current path of the path + * + * @return The array of currently active path points. + * + */ +const OpenSim::Array & PointBasedPath:: +getCurrentPath(const SimTK::State& s) const +{ + computePath(s); // compute checks if path needs to be recomputed + return getCacheVariableValue< Array >(s, "current_path"); +} + +//_____________________________________________________________________________ +/* + * Add a new path point, with default location, to the path. + * + * @param aIndex The position in the pathPointSet to put the new point in. + * @param frame The frame to attach the point to. + * @return Pointer to the newly created path point. + */ +AbstractPathPoint* PointBasedPath:: +addPathPoint(const SimTK::State& s, int aIndex, const PhysicalFrame& frame) +{ + PathPoint* newPoint = new PathPoint(); + newPoint->setParentFrame(frame); + Vec3 newLocation(0.0); + // Note: placeNewPathPoint() returns a location by reference. + // It computes a new location according to the index where the new path point + // will be inserted along the path(among the other path points). + placeNewPathPoint(s, newLocation, aIndex, frame); + // Now set computed new location into the newPoint + newPoint->setLocation(newLocation); + upd_PathPointSet().insert(aIndex, newPoint); + + // Rename the path points starting at this new one. + namePathPoints(aIndex); + + // Update start point and end point in the wrap instances so that they + // refer to the same path points they did before the new point + // was added. These indices are 1-based. + aIndex++; + for (int i=0; isetParentFrame(frame); + newPoint->setName(proposedName); + newPoint->setLocation(aPositionOnBody); + upd_PathPointSet().adoptAndAppend(newPoint); + + return newPoint; +} + +//_____________________________________________________________________________ +/* + * See if a path point can be deleted. All paths must have at least two + * active path points to define the path. + * + * @param aIndex The index of the point to delete. + * @return Whether or not the point can be deleted. + */ +bool PointBasedPath::canDeletePathPoint( int aIndex) +{ + // A path point can be deleted only if there would remain + // at least two other fixed points. + int numOtherFixedPoints = 0; + for (int i = 0; i < get_PathPointSet().getSize(); i++) { + if (i != aIndex) { + if (!( get_PathPointSet().get(i).getConcreteClassName() + ==("ConditionalPathPoint"))) + numOtherFixedPoints++; + } + } + + if (numOtherFixedPoints >= 2) + return true; + + return false; +} + +//_____________________________________________________________________________ +/* + * Delete a path point. + * + * @param aIndex The index of the point to delete. + * @return Whether or not the point was deleted. + */ +bool PointBasedPath::deletePathPoint(const SimTK::State& s, int aIndex) +{ + if (canDeletePathPoint(aIndex) == false) + return false; + + upd_PathPointSet().remove(aIndex); + + // rename the path points starting at the deleted position + namePathPoints(aIndex); + + // Update start point and end point in the wrap instances so that they + // refer to the same path points they did before the point was + // deleted. These indices are 1-based. If the point deleted is start + // point or end point, the path wrap range is made smaller by one point. + aIndex++; + for (int i=0; i get_PathPointSet().getSize())) + get_PathWrapSet().get(i).setStartPoint(s, startPoint - 1); + + if ( endPoint > 1 + && aIndex <= endPoint + && ( (endPoint > startPoint) + || (endPoint > get_PathPointSet().getSize()))) + get_PathWrapSet().get(i).setEndPoint(s, endPoint - 1); + } + + return true; +} + +//_____________________________________________________________________________ +/* + * Replace a path point in the set with another point. The new one is made a + * member of all the same groups as the old one, and is inserted in the same + * place the old one occupied. + * + * @param aOldPathPoint Path point to remove. + * @param aNewPathPoint Path point to add. + */ +bool PointBasedPath:: +replacePathPoint(const SimTK::State& s, AbstractPathPoint* aOldPathPoint, + AbstractPathPoint* aNewPathPoint) +{ + if (aOldPathPoint != NULL && aNewPathPoint != NULL) { + int count = 0; + int index = get_PathPointSet().getIndex(aOldPathPoint); + // If you're switching from non-via to via, check to make sure that the + // path will be left with at least 2 non-via points. + ConditionalPathPoint* oldVia = + dynamic_cast(aOldPathPoint); + ConditionalPathPoint* newVia = + dynamic_cast(aNewPathPoint); + if (oldVia == NULL && newVia != NULL) { + for (int i=0; i + (&get_PathPointSet().get(i)) == NULL) + count++; + } + } + } else { + count = 2; + } + if (count >= 2 && index >= 0) { + upd_PathPointSet().set(index, aNewPathPoint, true); + //computePath(s); + return true; + } + } + return false; +} + +//_____________________________________________________________________________ +/* + * Move a wrap instance up in the list. Changing the order of wrap instances for + * a path may affect how the path wraps over the wrap objects. + * + * @param aIndex The index of the wrap instance to move up. + */ +void PointBasedPath::moveUpPathWrap(const SimTK::State& s, int aIndex) +{ + if (aIndex > 0) { + // Make sure wrap object is not deleted by remove(). + upd_PathWrapSet().setMemoryOwner(false); + + PathWrap& wrap = get_PathWrapSet().get(aIndex); + upd_PathWrapSet().remove(aIndex); + upd_PathWrapSet().insert(aIndex - 1, &wrap); + upd_PathWrapSet().setMemoryOwner(true); + } +} + +//_____________________________________________________________________________ +/* + * Move a wrap instance down in the list. Changing the order of wrap instances + * for a path may affect how the path wraps over the wrap objects. + * + * @param aIndex The index of the wrap instance to move down. + */ +void PointBasedPath::moveDownPathWrap(const SimTK::State& s, int aIndex) +{ + if (aIndex < get_PathWrapSet().getSize() - 1) { + // Make sure wrap object is not deleted by remove(). + upd_PathWrapSet().setMemoryOwner(false); + + PathWrap& wrap = get_PathWrapSet().get(aIndex); + upd_PathWrapSet().remove(aIndex); + upd_PathWrapSet().insert(aIndex + 1, &wrap); + upd_PathWrapSet().setMemoryOwner(true); + } +} + +//_____________________________________________________________________________ +/* + * Delete a wrap instance. + * + * @param aIndex The index of the wrap instance to delete. + */ +void PointBasedPath::deletePathWrap(const SimTK::State& s, int aIndex) +{ + upd_PathWrapSet().remove(aIndex); + +} + +//_____________________________________________________________________________ +/* + * Apply the wrap objects to the current path. + */ +void PointBasedPath:: +applyWrapObjects(const SimTK::State& s, Array& path) const +{ + if (get_PathWrapSet().getSize() < 1) + return; + + WrapResult best_wrap; + Array result, order; + + result.setSize(get_PathWrapSet().getSize()); + order.setSize(get_PathWrapSet().getSize()); + + // Set the initial order to be the order they are listed in the path. + for (int i = 0; i < get_PathWrapSet().getSize(); i++) + order[i] = i; + + // If there is only one wrap object, calculate the wrapping only once. + // If there are two or more objects, perform up to 8 iterations where + // the result from one wrap object is used as the starting point for + // the next wrap. + const int maxIterations = get_PathWrapSet().getSize() < 2 ? 1 : 8; + double last_length = SimTK::Infinity; + for (int kk = 0; kk < maxIterations; kk++) + { + for (int i = 0; i < get_PathWrapSet().getSize(); i++) + { + result[i] = 0; + PathWrap& ws = get_PathWrapSet().get(order[i]); + const WrapObject* wo = ws.getWrapObject(); + best_wrap.wrap_pts.setSize(0); + double min_length_change = SimTK::Infinity; + + // First remove this object's wrapping points from the current path. + for (int j = 0; j get_active()) { + // startPoint and endPoint in wrapStruct represent the + // user-defined starting and ending points in the array of path + // points that should be considered for wrapping. These indices + // take into account via points, whether or not they are active. + // Thus they are indices into mp_orig[], not mp[] (also, mp[] + // may contain wrapping points from previous wrap objects, which + // would mess up the starting and ending indices). But the goal + // is to find starting and ending indices in mp[] to consider + // for wrapping over this wrap object. Here is how that is done: + + // 1. startPoint and endPoint are 1-based, so subtract 1 from + // them to get indices into get_PathPointSet(). -1 (or any value + // less than 1) means use the first (or last) point. + const int wrapStart = (ws.getStartPoint() < 1 + ? 0 + : ws.getStartPoint() - 1); + const int wrapEnd = (ws.getEndPoint() < 1 + ? get_PathPointSet().getSize() - 1 + : ws.getEndPoint() - 1); + + // 2. Scan forward from wrapStart in get_PathPointSet() to find + // the first point that is active. Store a pointer to it (smp). + int jfwd = wrapStart; + for (; jfwd <= wrapEnd; jfwd++) + if (get_PathPointSet().get(jfwd).isActive(s)) + break; + if (jfwd > wrapEnd) // there are no active points in the path + return; + const AbstractPathPoint* const smp = &get_PathPointSet().get(jfwd); + + // 3. Scan backwards from wrapEnd in get_PathPointSet() to find + // the last point that is active. Store a pointer to it (emp). + int jrev = wrapEnd; + for (; jrev >= wrapStart; jrev--) + if (get_PathPointSet().get(jrev).isActive(s)) + break; + if (jrev < wrapStart) // there are no active points in the path + return; + const AbstractPathPoint* const emp = &get_PathPointSet().get(jrev); + + // 4. Now find the indices of smp and emp in _currentPath. + int start=-1, end=-1; + for (int j = 0; j < path.getSize(); j++) { + if (path.get(j) == smp) + start = j; + if (path.get(j) == emp) + end = j; + } + if (start == -1 || end == -1) // this should never happen + return; + + // You now have indices into _currentPath (which is a list of + // all currently active points, including wrap points) that + // represent the used-defined range of points to consider for + // wrapping over this wrap object. Check each path segment in + // this range, choosing the best wrap as the one that changes + // the path segment length the least: + for (int pt1 = start; pt1 < end; pt1++) + { + const int pt2 = pt1 + 1; + + // As long as the two points are not auto wrap points on the + // same wrap object, check them for wrapping. + if ( path.get(pt1)->getWrapObject() == NULL + || path.get(pt2)->getWrapObject() == NULL + || ( path.get(pt1)->getWrapObject() + != path.get(pt2)->getWrapObject())) + { + WrapResult wr; + wr.startPoint = pt1; + wr.endPoint = pt2; + + result[i] = wo->wrapPathSegment(s, *path.get(pt1), + *path.get(pt2), ws, wr); + if (result[i] == WrapObject::mandatoryWrap) { + // "mandatoryWrap" means the path actually + // intersected the wrap object. In this case, you + // *must* choose this segment as the "best" one for + // wrapping. If the path has more than one segment + // that intersects the object, the first one is + // taken as the mandatory wrap (this is considered + // an ill-conditioned case). + best_wrap = wr; + // Store the best wrap in the pathWrap for possible + // use next time. + ws.setPreviousWrap(wr); + break; + } else if (result[i] == WrapObject::wrapped) { + // "wrapped" means the path segment was wrapped over + // the object, but you should consider the other + // segments as well to see if one + // wraps with a smaller length change. + double path_length_change = + calcPathLengthChange(s, *wo, wr, path); + if (path_length_change < min_length_change) + { + best_wrap = wr; + // Store the best wrap in the pathWrap for + // possible use next time + ws.setPreviousWrap(wr); + min_length_change = path_length_change; + } else { + // The wrap was not shorter than the current + // minimum, so just free the wrap points that + // were allocated. + wr.wrap_pts.setSize(0); + } + } else { + // Nothing to do. + } + } + } + + // Deallocate previous wrapping points if necessary. + ws.updWrapPoint2().getWrapPath().setSize(0); + + if (best_wrap.wrap_pts.getSize() == 0) { + ws.resetPreviousWrap(); + ws.updWrapPoint2().getWrapPath().setSize(0); + } else { + // If wrapping did occur, copy wrap info into the PathStruct. + ws.updWrapPoint1().getWrapPath().setSize(0); + + Array& wrapPath = ws.updWrapPoint2().getWrapPath(); + wrapPath = best_wrap.wrap_pts; + + // In OpenSim, all conversion to/from the wrap object's + // reference frame will be performed inside + // wrapPathSegment(). Thus, all points in this function will + // be in their respective body reference frames. + // for (j = 0; j < wrapPath.getSize(); j++){ + // convert_from_wrap_object_frame(wo, wrapPath.get(j)); + // convert(ms->modelnum, wrapPath.get(j), wo->segment, + // ms->ground_segment); + // } + + ws.updWrapPoint1().setWrapLength(0.0); + ws.updWrapPoint2().setWrapLength(best_wrap.wrap_path_length); + + ws.updWrapPoint1().setLocation(best_wrap.r1); + ws.updWrapPoint2().setLocation(best_wrap.r2); + + // Now insert the two new wrapping points into mp[] array. + path.insert(best_wrap.endPoint, &ws.updWrapPoint1()); + path.insert(best_wrap.endPoint + 1, &ws.updWrapPoint2()); + } + } + } + + const double length = calcLengthAfterPathComputation(s, path); + if (std::abs(length - last_length) < 0.0005) { + break; + } else { + last_length = length; + } + + if (kk == 0 && get_PathWrapSet().getSize() > 1) { + // If the first wrap was a no wrap, and the second was a no wrap + // because a point was inside the object, switch the order of + // the first two objects and try again. + if ( result[0] == WrapObject::noWrap + && result[1] == WrapObject::insideRadius) + { + order[0] = 1; + order[1] = 0; + + // remove wrap object 0 from the list of path points + PathWrap& ws = get_PathWrapSet().get(0); + for (int j = 0; j < path.getSize(); j++) { + if (path.get(j) == &ws.updWrapPoint1()) { + path.remove(j); // remove the first wrap point + path.remove(j); // remove the second wrap point + break; + } + } + } + } + } +} + +//_____________________________________________________________________________ +/* + * Name the path points based on their position in the set. To keep the + * names up to date, this method should be called every time the path changes. + * + * @param aStartingIndex The index of the first path point to name. + */ +void PointBasedPath::namePathPoints(int aStartingIndex) +{ + char indx[5]; + for (int i = aStartingIndex; i < get_PathPointSet().getSize(); i++) + { + sprintf(indx,"%d",i+1); + AbstractPathPoint& point = get_PathPointSet().get(i); + if (point.getName()=="" && hasOwner()) { + point.setName(getOwner().getName() + "-P" + indx); + } + } +} + +//_____________________________________________________________________________ +/* + * Determine an appropriate default XYZ location for a new path point. + * Note that this method is internal and should not be called directly on a new + * point as the point is not really added to the path (done in addPathPoint() + * instead) + * @param aOffset The XYZ location to be determined. + * @param aIndex The position in the pathPointSet to put the new point in. + * @param frame The body to attach the point to. + */ +void PointBasedPath:: +placeNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, int aIndex, + const PhysicalFrame& frame) +{ + // The location of the point is determined by moving a 'distance' from 'base' + // along a vector from 'start' to 'end.' 'base' is the existing path point + // that is in or closest to the index aIndex. 'start' and 'end' are existing + // path points--which ones depends on where the new point is being added. + // 'distance' is 0.5 for points added to the middle of a path (so the point + // appears halfway between the two adjacent points), and 0.2 for points that + // are added to either end of the path. If there is only one point in the + // path, the new point is put 0.01 units away in all three dimensions. + if (get_PathPointSet().getSize() > 1) { + int start, end, base; + double distance; + if (aIndex == 0) { + start = 1; + end = 0; + base = end; + distance = 0.2; + } else if (aIndex >= get_PathPointSet().getSize()) { + start = aIndex - 2; + end = aIndex - 1; + base = end; + distance = 0.2; + } else { + start = aIndex; + end = aIndex - 1; + base = start; + distance = 0.5; + } + + const Vec3 startPt = get_PathPointSet()[start].getLocation(s); + const Vec3 endPt = get_PathPointSet()[end].getLocation(s); + const Vec3 basePt = get_PathPointSet()[base].getLocation(s); + + Vec3 startPt2 = get_PathPointSet()[start].getParentFrame() + .findStationLocationInAnotherFrame(s, startPt, frame); + + Vec3 endPt2 = get_PathPointSet()[end].getParentFrame() + .findStationLocationInAnotherFrame(s, endPt, frame); + + aOffset = basePt + distance * (endPt2 - startPt2); + } else if (get_PathPointSet().getSize() == 1) { + aOffset= get_PathPointSet()[0].getLocation(s) + 0.01; + } + else { // first point, do nothing? + } +} + diff --git a/OpenSim/Simulation/Model/PointBasedPath.h b/OpenSim/Simulation/Model/PointBasedPath.h new file mode 100644 index 0000000000..07ec38604e --- /dev/null +++ b/OpenSim/Simulation/Model/PointBasedPath.h @@ -0,0 +1,133 @@ +#ifndef OPENSIM_POINTBASED_PATH_H_ +#define OPENSIM_POINTBASED_PATH_H_ + +/* -------------------------------------------------------------------------- * + * OpenSim: GeometryPath.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-2021 TU Delft and the Authors * + * Author(s): Joris Verhagen * + * * + * 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 + +#ifdef SWIG + #ifdef OSIMSIMULATION_API + #undef OSIMSIMULATION_API + #define OSIMSIMULATION_API + #endif +#endif + +// Forward declarations +namespace Simbody { +class State; +} + +namespace OpenSim { +class Coordinate; +} + +namespace OpenSim { + +/** + * An `OpenSim::GeometryPath` that is implemented as a sequence of points + * connected to their adjacent neighbors. + * + * Historically, in earlier versions of OpenSim, `OpenSim::GeometryPath`s were + * always implemented as `PointBasedPath`s. In later versions, there was a + * desire to define paths in terms of mathematical functions, curve fitting, + * lookups, etc., which is why the distinction now exists. + */ +class OSIMSIMULATION_API PointBasedPath : public GeometryPath { + OpenSim_DECLARE_CONCRETE_OBJECT(PointBasedPath, GeometryPath); + +public: + PointBasedPath(); + + double getLength(const SimTK::State& s) const override; + void setLength(const SimTK::State& s, double length) const override; + + double getLengtheningSpeed(const SimTK::State& s) const override; + void setLengtheningSpeed(const SimTK::State& s, double speed) const override; + + double computeMomentArm(const SimTK::State& s, + const Coordinate& aCoord) const override; + + + + + // From GeometryPath refactoring +public: + void addInEquivalentForces(const SimTK::State& state, + const double& tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const; + + void getPointForceDirections(const SimTK::State& s, + OpenSim::Array *rPFDs) const; + +protected: + void computePath(const SimTK::State& s ) const; + + double calcLengthAfterPathComputation + (const SimTK::State& s, const Array& currentPath) const; + + void generateDecorations( + bool fixed, + const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& appendToThis) const + override; + + // From GeometryPath directly related to points + + +protected: + double calcPathLengthChange(const SimTK::State& s, const WrapObject& wo, + const WrapResult& wr, + const Array& path) const; + void computeLengtheningSpeed(const SimTK::State& s) const; + +public: + void addPathWrap(WrapObject& aWrapObject); + +private: + const Array& getCurrentPath( const SimTK::State& s) const; + AbstractPathPoint* addPathPoint(const SimTK::State& s, int index, + const PhysicalFrame& frame); + AbstractPathPoint* appendNewPathPoint(const std::string& proposedName, + const PhysicalFrame& frame, const SimTK::Vec3& locationOnFrame); + bool canDeletePathPoint(int index); + bool deletePathPoint(const SimTK::State& s, int index); + bool replacePathPoint(const SimTK::State& s, + AbstractPathPoint* oldPathPoint, + AbstractPathPoint* newPathPoint); + + void moveUpPathWrap(const SimTK::State& s, int index); + void moveDownPathWrap(const SimTK::State& s, int index); + void deletePathWrap(const SimTK::State& s, int index); + + void applyWrapObjects(const SimTK::State& s, Array& path ) const; + + void namePathPoints(int aStartingIndex); + void placeNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, + int index, const PhysicalFrame& frame); +}; + +} +#endif diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 38b766acca..5a8124a1d9 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -59,7 +59,13 @@ #include "Model/PathPointSet.h" #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" + #include "Model/GeometryPath.h" +#include "Model/PointBasedPath.h" +#include "Model/FunctionBasedPathDiscretization.h" +#include "Model/FunctionBasedPathDiscretizationSet.h" +#include "Model/FunctionBasedPath.h" + #include "Model/PrescribedForce.h" #include "Model/ExternalForce.h" #include "Model/PointToPointSpring.h" @@ -188,7 +194,11 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( LineGeometry()); Object::registerType( FrameGeometry()); Object::registerType( Arrow()); - Object::registerType( GeometryPath()); + + Object::registerType( OpenSim::PointBasedPath()); + Object::registerType( OpenSim::FunctionBasedPathDiscretization()); + Object::registerType( OpenSim::FunctionBasedPathDiscretizationSet()); + Object::registerType( OpenSim::FunctionBasedPath()); Object::registerType( ControlSet() ); Object::registerType( ControlConstant() ); @@ -307,6 +317,9 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::renameType("MuscleMetabolicPowerProbeUmberger2010_MetabolicMuscleParameterSet", "Umberger2010MuscleMetabolicsProbe_MetabolicMuscleParameterSet"); + // GeometryPath now abstract but defaults to a PointBasedPath + Object::renameType("GeometryPath", "PointBasedPath"); + } catch (const std::exception& e) { std::cerr << "ERROR during osimSimulation Object registration:\n" diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 17b773ad80..6f25443a94 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -52,7 +52,13 @@ #include "Model/PathPointSet.h" #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" + #include "Model/GeometryPath.h" +#include "Model/PointBasedPath.h" +#include "Model/FunctionBasedPathDiscretization.h" +#include "Model/FunctionBasedPathDiscretizationSet.h" +#include "Model/FunctionBasedPath.h" + #include "Model/PrescribedForce.h" #include "Model/PointToPointSpring.h" #include "Model/ExpressionBasedPointToPointForce.h" diff --git a/OpenSim/Tools/FunctionBasedPathConversionTool.cpp b/OpenSim/Tools/FunctionBasedPathConversionTool.cpp new file mode 100644 index 0000000000..5311ccc407 --- /dev/null +++ b/OpenSim/Tools/FunctionBasedPathConversionTool.cpp @@ -0,0 +1,191 @@ +#include "FunctionBasedPathConversionTool.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +using namespace OpenSim; + +OpenSim::FunctionBasedPathConversionTool::FunctionBasedPathConversionTool() : + _modelPath{}, + _newModelName{}, + _params{}, + _verbose{false} +{ +} + +OpenSim::FunctionBasedPathConversionTool::FunctionBasedPathConversionTool( + const std::string& modelPath, + const std::string& newModelName) : + + _modelPath{modelPath}, + _newModelName{newModelName}, + _params{}, + _verbose{false} +{ +} + +const std::string& OpenSim::FunctionBasedPathConversionTool::getModelPath() const +{ + return _modelPath; +} + +void OpenSim::FunctionBasedPathConversionTool::setModelPath(const std::string& modelPath) +{ + _modelPath = modelPath; +} + +const std::string& OpenSim::FunctionBasedPathConversionTool::getNewModelName() const +{ + return _newModelName; +} + +void OpenSim::FunctionBasedPathConversionTool::setNewModelName(const std::string& newModelName) +{ + _newModelName = newModelName; +} + +const FunctionBasedPath::FittingParams& OpenSim::FunctionBasedPathConversionTool::getFittingParams() const { + return _params; +} + +void OpenSim::FunctionBasedPathConversionTool::setFittingParams(const FunctionBasedPath::FittingParams& p) { + _params = p; +} + +bool OpenSim::FunctionBasedPathConversionTool::getVerbose() const { + return _verbose; +} + +void OpenSim::FunctionBasedPathConversionTool::setVerbose(bool v) { + _verbose = v; +} + +bool OpenSim::FunctionBasedPathConversionTool::run() +{ + Model sourceModel{_modelPath}; + sourceModel.finalizeConnections(); + sourceModel.finalizeFromProperties(); + sourceModel.initSystem(); + + Model outputModel{sourceModel}; + outputModel.finalizeConnections(); + outputModel.finalizeFromProperties(); + + // print source model structure when runnning in verbose mode + if (_verbose) { + OpenSim::log_info("printing source model info"); + sourceModel.printSubcomponentInfo(); + } + + // struct that holds how a PBP in the source maps onto an actuator in the + // destination + struct PBPtoActuatorMapping final { + PointBasedPath& sourcePBP; + PathActuator& outputActuator; + + PBPtoActuatorMapping(PointBasedPath& sourcePBP_, + PathActuator& outputActuator_) : + sourcePBP{sourcePBP_}, + outputActuator{outputActuator_} { + } + }; + + // find PBPs in the source and figure out how they map onto `PathActuator`s + // in the destination + // + // (this is because `PathActuator`s are the "owners" of `GeometryPath`s in + // most models) + std::vector mappings; + for (PathActuator& pa : sourceModel.updComponentList()) { + + PointBasedPath* pbp = dynamic_cast(&pa.updGeometryPath()); + + // if the actuator doesn't use a PBP, ignore it + if (!pbp) { + continue; + } + + // otherwise, find the equivalent path in the destination + Component* c = const_cast(outputModel.findComponent(pa.getAbsolutePath())); + + if (!c) { + std::stringstream err; + err << "could not find '" << pa.getAbsolutePathString() << "' in the output model: this is a programming error"; + throw std::runtime_error{move(err).str()}; + } + + PathActuator* paDest = dynamic_cast(c); + + if (!paDest) { + std::stringstream err; + err << "the component '" << pa.getAbsolutePathString() << "' has a class of '" << pa.getConcreteClassName() << "' in the source model but a class of '" << c->getConcreteClassName() << "' in the destination model: this shouldn't happen"; + throw std::runtime_error{move(err).str()}; + } + + mappings.emplace_back(*pbp, *paDest); + } + + // for each `PathActuator` that uses a PBP, create an equivalent + // `FunctionBasedPath` (FBP) by fitting a function against the PBP and + // replace the PBP owned by the destination's `PathActuator` with the FBP + if (_verbose) { + OpenSim::log_info("using fitting params: maxCoordsThatCanAffectPath = {}, minProbingMomentArmChange = {}, numDiscretizationStepsPerDimension = {}, numProbingDiscretizations = {}", + _params.maxCoordsThatCanAffectPath, + _params.minProbingMomentArmChange, + _params.numDiscretizationStepsPerDimension, + _params.numProbingDiscretizations); + } + for (const PBPtoActuatorMapping& mapping : mappings) { + // create an FBP in-memory + + if (_verbose) { + OpenSim::log_info("attempting to convert point-based path '{}' into a FunctionBasedPath", mapping.sourcePBP.getAbsolutePathString()); + } + + std::unique_ptr maybeFbp = FunctionBasedPath::fromPointBasedPath(sourceModel, mapping.sourcePBP, _params); + + if (_verbose) { + if (maybeFbp) { + OpenSim::log_info("successfully converted '{}' into a FunctionBasedPath", mapping.sourcePBP.getAbsolutePathString()); + } else { + OpenSim::log_info("failed to convert '{}' into a FunctionBasedPath", mapping.sourcePBP.getAbsolutePathString()); + } + } + + if (maybeFbp) { + // assign the FBP over the destination's PBP + mapping.outputActuator.updProperty_GeometryPath().setValue(*maybeFbp); + } + } + + if (_verbose) { + OpenSim::log_info("finished attempting to fit all point based paths: emitting new output model that contains FunctionBasedPaths"); + } + + // the output model is now the same as the source model, but each PBP in + // its `PathActuator`s has been replaced with an FBP. Perform any final + // model-level fixups and save the output model. + outputModel.finalizeFromProperties(); + outputModel.finalizeConnections(); + outputModel.initSystem(); + outputModel.print(_newModelName); + + if (_verbose) { + OpenSim::log_info("--- interpolation complete ---"); + OpenSim::log_info("model before:"); + sourceModel.printSubcomponentInfo(); + OpenSim::log_info("model after:"); + outputModel.printSubcomponentInfo(); + } + + return true; +} diff --git a/OpenSim/Tools/FunctionBasedPathConversionTool.h b/OpenSim/Tools/FunctionBasedPathConversionTool.h new file mode 100644 index 0000000000..fcabeb67a0 --- /dev/null +++ b/OpenSim/Tools/FunctionBasedPathConversionTool.h @@ -0,0 +1,77 @@ +#ifndef __FunctionBasedPathConversionTool_h__ +#define __FunctionBasedPathConversionTool_h__ +/* -------------------------------------------------------------------------- * + * OpenSim: FunctionBasedPathConversionTool.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-2017 Stanford University and the Authors * + * Author(s): Joris Verhagen * + * * + * 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 + +#include "osimToolsDLL.h" + +#ifdef SWIG + #ifdef OSIMTOOLS_API + #undef OSIMTOOLS_API + #define OSIMTOOLS_API + #endif +#endif + +namespace OpenSim { +//============================================================================= +//============================================================================= +/** + * A concrete tool transforming a pointbasedpath model to a functionbasedpath + * model + * + * @author Joris Verhagen + * @version 1.0 + */ +class OSIMTOOLS_API FunctionBasedPathConversionTool: public AbstractTool { + + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathConversionTool, AbstractTool); + +private: + std::string _modelPath; + std::string _newModelName; + FunctionBasedPath::FittingParams _params; + bool _verbose; + +public: + FunctionBasedPathConversionTool(); + FunctionBasedPathConversionTool(const std::string& modelPath, const std::string& newModelName); + + const std::string& getModelPath() const; + void setModelPath(const std::string&); + + const std::string& getNewModelName() const; + void setNewModelName(const std::string&); + + const FunctionBasedPath::FittingParams& getFittingParams() const; + void setFittingParams(const FunctionBasedPath::FittingParams&); + + bool getVerbose() const; + void setVerbose(bool); + + bool run() override SWIG_DECLARE_EXCEPTION; +}; +} +#endif // _FunctionBasedPathConversionTool_h__ + + diff --git a/OpenSim/Tools/RegisterTypes_osimTools.cpp b/OpenSim/Tools/RegisterTypes_osimTools.cpp index 6def930d80..27a03342bc 100644 --- a/OpenSim/Tools/RegisterTypes_osimTools.cpp +++ b/OpenSim/Tools/RegisterTypes_osimTools.cpp @@ -34,6 +34,7 @@ #include "AnalyzeTool.h" #include "InverseKinematicsTool.h" #include "IMUInverseKinematicsTool.h" +#include "FunctionBasedPathConversionTool.h" #include "InverseDynamicsTool.h" @@ -79,6 +80,7 @@ OSIMTOOLS_API void RegisterTypes_osimTools() Object::registerType( RRATool() ); Object::registerType( ForwardTool() ); Object::registerType( AnalyzeTool() ); + Object::registerType( FunctionBasedPathConversionTool() ); Object::registerType( GenericModelMaker() ); Object::registerType( IKCoordinateTask() ); @@ -125,4 +127,4 @@ osimToolsInstantiator::osimToolsInstantiator() void osimToolsInstantiator::registerDllClasses() { RegisterTypes_osimTools(); -} \ No newline at end of file +} diff --git a/OpenSim/Tools/osimTools.h b/OpenSim/Tools/osimTools.h index 66b7c3b889..bf923c8c93 100644 --- a/OpenSim/Tools/osimTools.h +++ b/OpenSim/Tools/osimTools.h @@ -28,6 +28,7 @@ #include "CMCTool.h" #include "ForwardTool.h" #include "AnalyzeTool.h" +#include "FunctionBasedPathConversionTool.h" #include "InverseKinematicsTool.h" #include "InverseDynamicsTool.h"