diff --git a/Bindings/OpenSimHeaders_simulation.h b/Bindings/OpenSimHeaders_simulation.h index f22f948d4c..7a9870562d 100644 --- a/Bindings/OpenSimHeaders_simulation.h +++ b/Bindings/OpenSimHeaders_simulation.h @@ -110,6 +110,8 @@ #include #include #include +#include +#include #include #include diff --git a/OpenSim/Simulation/Model/Appearance.h b/OpenSim/Simulation/Model/Appearance.h index d1663a3040..9c09b2d1a4 100644 --- a/OpenSim/Simulation/Model/Appearance.h +++ b/OpenSim/Simulation/Model/Appearance.h @@ -28,6 +28,7 @@ #include #include #include +#include namespace OpenSim { diff --git a/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp b/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp index 5d68506aad..5acb8042da 100644 --- a/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp +++ b/OpenSim/Simulation/Model/Blankevoort1991Ligament.cpp @@ -21,9 +21,11 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ +#include "Blankevoort1991Ligament.h" + #include +#include #include -#include "Blankevoort1991Ligament.h" using namespace OpenSim; @@ -81,7 +83,7 @@ void Blankevoort1991Ligament::setNull() } void Blankevoort1991Ligament::constructProperties() { - constructProperty_GeometryPath(GeometryPath()); + constructProperty_GeometryPath(PointBasedPath{}); 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..1ba07312f8 --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -0,0 +1,321 @@ +#include "FunctionBasedPath.h" + +#include +#include +#include + +#include + +namespace { + OpenSim::Exception InvalidFunctionError(std::string parentPath, char const* funcName) + { + std::stringstream ss; + ss << parentPath << ':' << funcName << ": cannot call underlying function: it has not been initialized yet: ensure you have assigned a function to this FunctionBasedPath and finalized its properites"; + return OpenSim::Exception{ss.str()}; + } + + // stub function that will throw an exception with an information message if called + // + // used as a stand-in for the object state where the caller has allocated a FunctionBasedPath + // but hasn't set its underlying function yet. + class ThrowingSimTKFunction final : public SimTK::Function { + std::string m_ParentPath; + public: + explicit ThrowingSimTKFunction(const std::string& parentPath) : + m_ParentPath{parentPath} + { + } + + double calcValue(const SimTK::Vector&) const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + + double calcDerivative(const SimTK::Array_&, const SimTK::Vector&) const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + + int getArgumentSize() const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + + int getMaxDerivativeOrder() const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + }; + + // stub function that will throw an exception with an information message if called + // + // used as a stand-in for the object state where the caller has allocated a FunctionBasedPath + // but hasn't set its underlying function yet. + class ThrowingOpenSimFunction final : public OpenSim::Function { + OpenSim_DECLARE_CONCRETE_OBJECT(ThrowingOpenSimFunction, OpenSim::Function) + + std::string m_ParentPath; + public: + explicit ThrowingOpenSimFunction(const std::string& parentPath) : + m_ParentPath{parentPath} + { + } + + double calcValue(const SimTK::Vector&) const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + + double calcDerivative(const std::vector&, const SimTK::Vector&) const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + + int getArgumentSize() const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + + int getMaxDerivativeOrder() const override + { + throw InvalidFunctionError(m_ParentPath, __func__); + } + + SimTK::Function* createSimTKFunction() const override + { + return new ThrowingSimTKFunction{m_ParentPath}; + } + }; + + void ValidatePropertiesOrThrow(const OpenSim::FunctionBasedPath& parent, + const OpenSim::Property& funcProp, + const OpenSim::Property& coordPathsProp) + { + if (funcProp.size() == 0) { + throw OpenSim::Exception(__FILE__, __LINE__, __func__, parent, "A FunctionBasedPath's function property has not been set"); + } + + if (funcProp.getValue().getMaxDerivativeOrder() < 1) { + throw OpenSim::Exception(__FILE__, __LINE__, __func__, parent, "The function provided to a FunctionBasedPath is not differentiable: it cannot be used as a path function"); + } + + if (funcProp.getValue().getArgumentSize() != coordPathsProp.size()) { + std::stringstream ss; + ss << "The number of coordinate paths provided (" << coordPathsProp.size() << ") does not match the number of arguments the function, " << funcProp.getValue().getName() << ", takes (" << funcProp.getValue().getArgumentSize() << ")"; + throw OpenSim::Exception(__FILE__, __LINE__, __func__, parent, ss.str()); + } + + int nCoords = coordPathsProp.size(); + for (int i = 0; i < nCoords; ++i) { + const std::string& coordPath = coordPathsProp.getValue(i); + if (coordPath.empty()) { + throw OpenSim::Exception(__FILE__, __LINE__, __func__, parent, "An empty coordinate string was provided to a FunctionBasedPath: all coordinate strings must be absolute paths to coordinates within the model"); + } + } + } +} + +OpenSim::FunctionBasedPath::FunctionBasedPath() : GeometryPath{} +{ + constructProperty_PathFunction(ThrowingOpenSimFunction{this->getAbsolutePathString()}); + constructProperty_Coordinates(); +} + +OpenSim::FunctionBasedPath::FunctionBasedPath(const FunctionBasedPath&) = default; + +OpenSim::FunctionBasedPath::FunctionBasedPath(const OpenSim::Function& func, std::vector coordAbsPaths) +{ + constructProperty_PathFunction(func); + constructProperty_Coordinates(); + for (std::string& coordAbsPath : coordAbsPaths) { + updProperty_Coordinates().appendValue(std::move(coordAbsPath)); + } + + ValidatePropertiesOrThrow(*this, getProperty_PathFunction(), getProperty_Coordinates()); +} + +OpenSim::FunctionBasedPath::~FunctionBasedPath() noexcept = default; + +OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(const FunctionBasedPath&) = default; + +SimTK::Vec3 OpenSim::FunctionBasedPath::getColor(const SimTK::State& s) const +{ + return getCacheVariableValue(s, _colorCV); +} + +void OpenSim::FunctionBasedPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const +{ + setCacheVariableValue(s, _colorCV, color); +} + +double OpenSim::FunctionBasedPath::getLength(const SimTK::State& s) const +{ + if (isCacheVariableValid(s, _lengthCV)) { + return getCacheVariableValue(s, _lengthCV); + } + + const SimTK::Vector& args = calcFunctionArguments(s); + double v = getProperty_PathFunction().getValue().calcValue(args); + setCacheVariableValue(s, _lengthCV, v); + return v; +} + +double OpenSim::FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const +{ + if (isCacheVariableValid(s, _speedCV)) { + return getCacheVariableValue(s, _speedCV); + } + + // the lengthening speed is the sum of: pathDeriv (w.r.t. coord) * coordinateSpeed + _derivativeOrderBuffer.resize(1); + const SimTK::Vector& args = calcFunctionArguments(s); + int nCoords = getProperty_Coordinates().size(); + double acc = 0.0; + for (int i = 0; i < nCoords; ++i) { + const std::string& coordAbsPath = get_Coordinates(i); + const OpenSim::Coordinate& coord = getRoot().getComponent(coordAbsPath); + _derivativeOrderBuffer[0] = i; + double deriv = get_PathFunction().calcDerivative(_derivativeOrderBuffer, args); + double coordSpeed = coord.getSpeedValue(s); + + acc = acc + deriv*coordSpeed; + } + setCacheVariableValue(s, _speedCV, acc); + return acc; +} + +void OpenSim::FunctionBasedPath::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_&, SimTK::Vector& mobilityForces) const +{ + const SimTK::SimbodyMatterSubsystem& matter = getModel().getMatterSubsystem(); + + const SimTK::Vector args = calcFunctionArguments(state); + + _derivativeOrderBuffer.resize(1); + int nCoords = getProperty_Coordinates().size(); + for (int i = 0; i < nCoords; ++i) { + const std::string& coordAbsPath = get_Coordinates(i); + const OpenSim::Coordinate& coord = getRoot().getComponent(coordAbsPath); + _derivativeOrderBuffer[0] = i; + double momentArm = get_PathFunction().calcDerivative(_derivativeOrderBuffer, args); + + double torque = -tension*momentArm; + + matter.addInMobilityForce(state, + SimTK::MobilizedBodyIndex(coord.getBodyIndex()), + SimTK::MobilizerUIndex(coord.getMobilizerQIndex()), + torque, + mobilityForces); + } +} + +double OpenSim::FunctionBasedPath::computeMomentArm(const SimTK::State& st, const Coordinate& coord) const +{ + // the moment arm of a path with respect to a coordinate is the path's + // length derivative with respect to the coordinate + + int coordIndex = indexOfCoordinate(coord); + if (coordIndex == -1) { + // the provided coordinate does not affect this path, so it + // has no moment arm w.r.t. it + return 0.0; + } + + _derivativeOrderBuffer.resize(1); + _derivativeOrderBuffer[0] = coordIndex; + + const SimTK::Vector& args = calcFunctionArguments(st); + + return getProperty_PathFunction().getValue().calcDerivative(_derivativeOrderBuffer, args); +} + +void OpenSim::FunctionBasedPath::extendFinalizeFromProperties() +{ + ValidatePropertiesOrThrow(*this, getProperty_PathFunction(), getProperty_Coordinates()); +} + +void OpenSim::FunctionBasedPath::extendAddToSystem(SimTK::MultibodySystem& system) const +{ + Super::extendAddToSystem(system); + + // Allocate cache entries to save the current length and speed(=d/dt length) + // of the path in the cache. Length depends only on q's so will be valid + // after Position stage, speed requires u's also so valid at Velocity stage. + this->_lengthCV = addCacheVariable("length", 0.0, SimTK::Stage::Position); + this->_speedCV = addCacheVariable("speed", 0.0, SimTK::Stage::Velocity); + + // We consider this cache entry valid any time after it has been created + // and first marked valid, and we won't ever invalidate it. + this->_colorCV = addCacheVariable("color", get_Appearance().get_color(), SimTK::Stage::Topology); +} + +void OpenSim::FunctionBasedPath::extendInitStateFromProperties(SimTK::State& s) const +{ + Super::extendInitStateFromProperties(s); + markCacheVariableValid(s, _colorCV); +} + +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); + } + } + + int nCoords = getProperty_Coordinates().size(); + for (int i = 0; i < nCoords; ++i) { + root.getComponent(get_Coordinates(i)); // should throw if missing + } +} + +const SimTK::Vector& OpenSim::FunctionBasedPath::calcFunctionArguments(const SimTK::State& st) const +{ + int nargs = getProperty_Coordinates().size(); + _functionArgsBuffer.resize(nargs); + for (int i = 0; i < nargs; ++i) { + // HACK: this lookup is horrible + const std::string& coordName = get_Coordinates(i); + const OpenSim::Coordinate& coord = getRoot().getComponent(coordName); + _functionArgsBuffer[i] = coord.getValue(st); + } + return _functionArgsBuffer; +} + +int OpenSim::FunctionBasedPath::indexOfCoordinate(const Coordinate& c) const +{ + std::string absPath = c.getAbsolutePathString(); + + int nCoords = getProperty_Coordinates().size(); + for (int i = 0; i < nCoords; ++i) { + const std::string& coordAbsPath = get_Coordinates(i); + if (coordAbsPath == absPath) { + return i; + } + } + return -1; +} + +/* TODO: in extendFinalizeConnections + * +// 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(JorisFBP& impl, + OpenSim::Component const& c) { + + for (size_t i = 0; i < impl.coords.size(); ++i) { + impl.coords[i] = &c.getComponent(impl.coordAbsPaths[i]); + } +} +*/ diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h new file mode 100644 index 0000000000..954efc0629 --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -0,0 +1,86 @@ +#ifndef OPENSIM_FUNCTIONBASED_PATH_H_ +#define OPENSIM_FUNCTIONBASED_PATH_H_ + +#include +#include +#include +#include +#include +#include + +#ifdef SWIG + #ifdef OSIMSIMULATION_API + #undef OSIMSIMULATION_API + #define OSIMSIMULATION_API + #endif +#endif + +namespace SimTK { +class State; +} + +namespace OpenSim { + +class Coordinate; +class PointForceDirection; + +/** + * An `OpenSim::GeometryPath` that uses `PathFunction`s to compute its state. + * + * A `FunctionBasedPath` uses an externally-provided `PathFunction` to compute the + * actual path, and also handles any `GeometryPath`-specific concerns, such as + * handling coloring and caching results. + */ +class OSIMSIMULATION_API FunctionBasedPath final : public GeometryPath { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, GeometryPath); + + OpenSim_DECLARE_PROPERTY(PathFunction, OpenSim::Function, "The underlying function that is used at simulation-time to evaluate the length and lengthening speed (derivative) of the path. The function's arity (and argument order) is equal to the coordinates that affect the path. At evaluation-time, the function (value + derivative) is called with a sequence of the coordinate values (the values of each coordinate in the current state)"); + OpenSim_DECLARE_LIST_PROPERTY(Coordinates, std::string, "Absolute paths to coordinates in the model. The number of coordinates provided must equal the arity of the function property. At simulation-time, each of these coordinates are looked up to get their values, which are pumped into the path evaluation function"); + + mutable CacheVariable _lengthCV; + mutable CacheVariable _speedCV; + mutable CacheVariable _colorCV; + mutable SimTK::Vector _functionArgsBuffer; + mutable std::vector _derivativeOrderBuffer; + +public: + FunctionBasedPath(); + FunctionBasedPath(const FunctionBasedPath&); + + /** + * Construct the FunctionBasedPath and immediately assign its PathFunction and + * Coordinates list. + * + * The length of the coordinates list must match the artity (getArgumentSize) + * of the function. The function must be differentiable. + */ + FunctionBasedPath(const OpenSim::Function&, std::vector coordAbsPaths); + ~FunctionBasedPath() noexcept; + + FunctionBasedPath& operator=(FunctionBasedPath const&); + + SimTK::Vec3 getColor(const SimTK::State& s) const override; + void setColor(const SimTK::State& s, const SimTK::Vec3& color) const override; + + double getLength(const SimTK::State& s) const override; + double getLengtheningSpeed(const SimTK::State& s) const override; + + void addInEquivalentForces(const SimTK::State& state, + double tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const override; + + double computeMomentArm(const SimTK::State& s, + const Coordinate& aCoord) const override; + + void extendFinalizeFromProperties() override; + void extendAddToSystem(SimTK::MultibodySystem& system) const override; + void extendInitStateFromProperties(SimTK::State& s) const override; + void extendFinalizeConnections(OpenSim::Component&) override; +private: + const SimTK::Vector& calcFunctionArguments(const SimTK::State&) const; + int indexOfCoordinate(const Coordinate&) const; +}; +} + +#endif // FUNCTIONBASEDPATH_H diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 2953703bb7..f4066eebcd 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -21,1173 +21,283 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================= -// INCLUDES -//============================================================================= #include "GeometryPath.h" -#include "ConditionalPathPoint.h" -#include "MovingPathPoint.h" -#include "PointForceDirection.h" -#include -#include "Model.h" - -//============================================================================= -// STATICS -//============================================================================= -using namespace std; -using namespace OpenSim; -using namespace SimTK; -using SimTK::Vec3; - -//============================================================================= -// CONSTRUCTOR(S) AND DESTRUCTOR -//============================================================================= -//_____________________________________________________________________________ -/* - * Default constructor. - */ -GeometryPath::GeometryPath() : - ModelComponent(), - _preScaleLength(0.0) -{ - setAuthors("Peter Loan"); - constructProperties(); - } - -//_____________________________________________________________________________ -/* -* Perform set up functions after model has been deserialized or copied. -* -*/ -void GeometryPath::extendFinalizeFromProperties() -{ - Super::extendFinalizeFromProperties(); - for (int i = 0; i < get_PathWrapSet().getSize(); ++i) { - if (upd_PathWrapSet()[i].getName().empty()) { - std::stringstream label; - label << "pathwrap_" << i; - upd_PathWrapSet()[i].setName(label.str()); - } - } -} +#include -void GeometryPath::extendConnectToModel(Model& aModel) -{ - Super::extendConnectToModel(aModel); - OPENSIM_THROW_IF_FRMOBJ(get_PathPointSet().getSize() < 2, - InvalidPropertyValue, - getProperty_PathPointSet().getName(), - "A valid path must be connected to a model by at least two PathPoints.") +class OpenSim::GeometryPath::Impl final { +public: + // used by `(get|set)PreLengthScale`. Used during `extend(Pre|Post)Scale` by + // downstream users of GeometryPath to cache the length of the path before + // scaling. + // + // Ideally, downstream classes would perform the caching themselves, because + // the GeometryPath API isn't an ideal place to store this information. This + // field is mostly here for backwards-compatability with the API. + double _preScaleLength = 0.0; - // Name the path points based on the current path - // (i.e., the set of currently active points is numbered - // 1, 2, 3, ...). - namePathPoints(0); -} +private: + // used whenever something sneaky is happening, like when a mutation is happening + // underneath a const method. Multiple threads *should* be able to use the `const` + // API without any concern about races. + SimTK::ResetOnCopy constCastMutex; -//_____________________________________________________________________________ -/* - * Create the SimTK state, discrete and/or cache for this GeometryPath. - */ - void GeometryPath::extendAddToSystem(SimTK::MultibodySystem& system) const -{ - Super::extendAddToSystem(system); + // used as a stand-in if the caller uses the (deprecated) point-based API + SimTK::ClonePtr fixupPathPointSet = nullptr; - // Allocate cache entries to save the current length and speed(=d/dt length) - // of the path in the cache. Length depends only on q's so will be valid - // after Position stage, speed requires u's also so valid at Velocity stage. - this->_lengthCV = addCacheVariable("length", 0.0, SimTK::Stage::Position); - this->_speedCV = addCacheVariable("speed", 0.0, SimTK::Stage::Velocity); + // used as a stand-in if the caller uses the (deprecated) point-based API + SimTK::ClonePtr fixupPathPoint = nullptr; - // Cache the set of points currently defining this path. - this->_currentPathCV = addCacheVariable("current_path", Array{}, SimTK::Stage::Position); + // used as a stand-in if the caller uses the (deprecated) point-based API + OpenSim::Array fixupCurrentPathArray; - // We consider this cache entry valid any time after it has been created - // and first marked valid, and we won't ever invalidate it. - this->_colorCV = addCacheVariable("color", get_Appearance().get_color(), SimTK::Stage::Topology); -} + // used as a stand-in if the caller uses the (deprecated) point-based API + SimTK::ClonePtr fixupPathWrapSet = nullptr; - void GeometryPath::extendInitStateFromProperties(SimTK::State& s) const -{ - Super::extendInitStateFromProperties(s); - markCacheVariableValid(s, _colorCV); // it is OK at its default value -} +public: + Impl* clone() const { + return new Impl{*this}; + } -//------------------------------------------------------------------------------ -// 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; + OpenSim::PathPointSet& getOrUpdPPSCached() const + { + if (!fixupPathPointSet) { + std::lock_guard guard{const_cast(*this).constCastMutex.updT()}; + const_cast(*this).fixupPathPointSet = new OpenSim::PathPointSet{}; } + return *const_cast(*this).fixupPathPointSet; } -} - -//_____________________________________________________________________________ -/* - * Connect properties to local pointers. - */ -void GeometryPath::constructProperties() -{ - constructProperty_PathPointSet(PathPointSet()); - - constructProperty_PathWrapSet(PathWrapSet()); - - Appearance appearance; - appearance.set_color(SimTK::Gray); - 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++) + OpenSim::AbstractPathPoint* getOrUpdFixupPathPointCached() const { - sprintf(indx,"%d",i+1); - AbstractPathPoint& point = get_PathPointSet().get(i); - if (point.getName()=="" && hasOwner()) { - point.setName(getOwner().getName() + "-P" + indx); + if (!fixupPathPoint) { + std::lock_guard guard{const_cast(*this).constCastMutex.updT()}; + const_cast(*this).fixupPathPoint = new OpenSim::PathPoint{}; } + return const_cast(*this).fixupPathPoint.upd(); } -} -//_____________________________________________________________________________ -/* - * 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); + OpenSim::Array& getOrUpdCurrentPathArray() const + { + return const_cast(*this).fixupCurrentPathArray; } - 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); + PathWrapSet& getOrUpdPathWrapSet() const + { + if (!fixupPathWrapSet) { + std::lock_guard guard{const_cast(*this).constCastMutex.updT()}; + const_cast(*this).fixupPathWrapSet = new OpenSim::PathWrapSet{}; } - } -} -/* 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); - } - } + return *const_cast(*this).fixupPathWrapSet.upd(); } -} +}; -//_____________________________________________________________________________ -/* - * Update the geometric representation of the path. - * The resulting geometry is maintained at the VisibleObject layer. - * This function should not be made public. It is called internally - * by compute() only when the path has changed. - * - */ -void GeometryPath::updateGeometry(const SimTK::State& s) const +OpenSim::GeometryPath::GeometryPath() : ModelComponent{}, _impl{new Impl{}} { - // Check if the current path needs to recomputed. - computePath(s); -} + setAuthors("Peter Loan"); -//============================================================================= -// 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); + Appearance appearance; + appearance.set_color(SimTK::Gray); + constructProperty_Appearance(appearance); } -void GeometryPath::setLength( const SimTK::State& s, double length ) const +OpenSim::GeometryPath::GeometryPath(OpenSim::GeometryPath const&) = default; + +OpenSim::GeometryPath::~GeometryPath() noexcept = default; + +OpenSim::GeometryPath& OpenSim::GeometryPath::operator=(const GeometryPath&) = default; + + +// DEFAULTED METHODS + +const SimTK::Vec3& OpenSim::GeometryPath::getDefaultColor() const { - setCacheVariableValue(s, _lengthCV, length); + return get_Appearance().get_color(); } -void GeometryPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const +void OpenSim::GeometryPath::setDefaultColor(const SimTK::Vec3& color) { - setCacheVariableValue(s, _colorCV, color); + updProperty_Appearance().setValueIsDefault(false); + upd_Appearance().set_color(color); } -Vec3 GeometryPath::getColor(const SimTK::State& s) const +double OpenSim::GeometryPath::getPreScaleLength(const SimTK::State&) const { - return getCacheVariableValue(s, _colorCV); + return _impl->_preScaleLength; } -//_____________________________________________________________________________ -/* - * Compute the lengthening speed of the path. - * - * @return lengthening speed of the path. - */ -double GeometryPath::getLengtheningSpeed( const SimTK::State& s) const +void OpenSim::GeometryPath::setPreScaleLength(const SimTK::State&, double preScaleLength) { - computeLengtheningSpeed(s); - return getCacheVariableValue(s, _speedCV); + _impl->_preScaleLength = preScaleLength; } -void GeometryPath::setLengtheningSpeed( const SimTK::State& s, double speed ) const + + +// SHIM/DEPRECATED METHODS +// +// we provide a "shim" implementation of the deprecated API. It isn't logically +// valid, or useful, but providing it here means that other downstream classes +// (i.e. ones that do not use point-based implementations) do not have to +// provide their own shims around a now-deprecated API. +// +// The name of the game here is "crash stabiliity", not "validity". This code +// just needs to ensure that calling code that uses the legacy API doesn't +// segfault or explode when that calling code uses the (deprecated) point-based +// API with a not-point-based (e.g. functional) path implementation. The +// implementations here just need to be crash-stable and (ideally) print out +// a runtime warning that informs the user/developer that they're using shimmed +// (and therefore, probably invalid) code. + +static bool emitDeprecationWarning(char const* funcName) { - setCacheVariableValue(s, _speedCV, speed); + OpenSim::log_warn("deprecation warning: OpenSim tried to call '{}', which is now deprecated. This may result in unusual outputs", funcName); + return true; } -void GeometryPath::setPreScaleLength( const SimTK::State& s, double length ) { - _preScaleLength = length; -} -double GeometryPath::getPreScaleLength( const SimTK::State& s) const { - return _preScaleLength; -} +#define SHOW_FUNC_DEPRECATION_WARNING() \ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); \ + (void)g_ShownDeprecationWarning; -//============================================================================= -// 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) +const OpenSim::PathPointSet& OpenSim::GeometryPath::getPathPointSet() const { - 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; igetOrUpdPPSCached(); } -AbstractPathPoint* GeometryPath:: -appendNewPathPoint(const std::string& proposedName, - const PhysicalFrame& frame, - const SimTK::Vec3& aPositionOnBody) +OpenSim::PathPointSet& OpenSim::GeometryPath::updPathPointSet() { - PathPoint* newPoint = new PathPoint(); - newPoint->setParentFrame(frame); - newPoint->setName(proposedName); - newPoint->setLocation(aPositionOnBody); - upd_PathPointSet().adoptAndAppend(newPoint); + SHOW_FUNC_DEPRECATION_WARNING(); - return newPoint; + return _impl->getOrUpdPPSCached(); } -//_____________________________________________________________________________ -/* - * 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) +OpenSim::AbstractPathPoint* OpenSim::GeometryPath::addPathPoint( + const SimTK::State&, + int, + const PhysicalFrame&) { - // 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); + SHOW_FUNC_DEPRECATION_WARNING(); - Vec3 startPt2 = get_PathPointSet()[start].getParentFrame() - .findStationLocationInAnotherFrame(s, startPt, frame); + // don't actually add the path point, just return a dummy path point to + // ensure downstream code reliant on the deprecated API doesn't explode + // with a segfault + return _impl->getOrUpdFixupPathPointCached(); +} - Vec3 endPt2 = get_PathPointSet()[end].getParentFrame() - .findStationLocationInAnotherFrame(s, endPt, frame); +OpenSim::AbstractPathPoint* OpenSim::GeometryPath::appendNewPathPoint( + const std::string&, + const PhysicalFrame&, + const SimTK::Vec3&) +{ + SHOW_FUNC_DEPRECATION_WARNING(); - aOffset = basePt + distance * (endPt2 - startPt2); - } else if (get_PathPointSet().getSize() == 1) { - aOffset= get_PathPointSet()[0].getLocation(s) + 0.01; - } - else { // first point, do nothing? - } + // don't actually add the path point, just return a dummy path point to + // ensure downstream code reliant on the deprecated API doesn't explode + // with a segfault + return _impl->getOrUpdFixupPathPointCached(); } -//_____________________________________________________________________________ -/* - * 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) +bool OpenSim::GeometryPath::canDeletePathPoint(int) { - // 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; + SHOW_FUNC_DEPRECATION_WARNING(); 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) +bool OpenSim::GeometryPath::deletePathPoint(const SimTK::State&, int) { - 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); - } + SHOW_FUNC_DEPRECATION_WARNING(); - return true; + return false; } -//_____________________________________________________________________________ -/* - * 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) +bool OpenSim::GeometryPath::replacePathPoint( + const SimTK::State&, + AbstractPathPoint*, + AbstractPathPoint*) { - 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; - } - } + SHOW_FUNC_DEPRECATION_WARNING(); + 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) +const OpenSim::Array& OpenSim::GeometryPath::getCurrentPath(const SimTK::State&) const { - PathWrap* newWrap = new PathWrap(); - newWrap->setWrapObject(aWrapObject); - newWrap->setMethod(PathWrap::hybrid); - upd_PathWrapSet().adoptAndAppend(newWrap); - finalizeFromProperties(); -} + SHOW_FUNC_DEPRECATION_WARNING(); -//_____________________________________________________________________________ -/* - * 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); - } + return _impl->getOrUpdCurrentPathArray(); } -//_____________________________________________________________________________ -/* - * 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) +const OpenSim::PathWrapSet& OpenSim::GeometryPath::getWrapSet() const { - 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); - } + SHOW_FUNC_DEPRECATION_WARNING(); + + return _impl->getOrUpdPathWrapSet(); } -//_____________________________________________________________________________ -/* - * Delete a wrap instance. - * - * @param aIndex The index of the wrap instance to delete. - */ -void GeometryPath::deletePathWrap(const SimTK::State& s, int aIndex) +OpenSim::PathWrapSet& OpenSim::GeometryPath::updWrapSet() { - upd_PathWrapSet().remove(aIndex); + SHOW_FUNC_DEPRECATION_WARNING(); + return _impl->getOrUpdPathWrapSet(); } -//============================================================================== -// SCALING -//============================================================================== -void GeometryPath:: -extendPreScale(const SimTK::State& s, const ScaleSet& scaleSet) +void OpenSim::GeometryPath::addPathWrap(WrapObject&) { - Super::extendPreScale(s, scaleSet); - setPreScaleLength(s, getLength(s)); -} + SHOW_FUNC_DEPRECATION_WARNING(); -void GeometryPath:: -extendPostScale(const SimTK::State& s, const ScaleSet& scaleSet) -{ - Super::extendPostScale(s, scaleSet); - computePath(s); + return; } -//-------------------------------------------------------------------------- -// COMPUTATIONS -//-------------------------------------------------------------------------- -//============================================================================= -// PATH, WRAPPING, AND MOMENT ARM -//============================================================================= -//_____________________________________________________________________________ -/* - * Calculate the current path. - */ -void GeometryPath::computePath(const SimTK::State& s) const +void OpenSim::GeometryPath::moveUpPathWrap(const SimTK::State&, int) { - 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); + SHOW_FUNC_DEPRECATION_WARNING(); - markCacheVariableValid(s, _currentPathCV); + return; } -//_____________________________________________________________________________ -/* - * Compute lengthening speed of the path. - */ -void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const +void OpenSim::GeometryPath::moveDownPathWrap(const SimTK::State&, int) { - if (isCacheVariableValid(s, _speedCV)) { - return; - } - - const Array& currentPath = getCurrentPath(s); + SHOW_FUNC_DEPRECATION_WARNING(); - double speed = 0.0; - - for (int i = 0; i < currentPath.getSize() - 1; i++) { - speed += currentPath[i]->calcSpeedBetween(s, *currentPath[i+1]); - } - - setLengtheningSpeed(s, speed); + return; } -//_____________________________________________________________________________ -/* - * Apply the wrap objects to the current path. - */ -void GeometryPath:: -applyWrapObjects(const SimTK::State& s, Array& path) const +void OpenSim::GeometryPath::deletePathWrap(const SimTK::State&, int) { - 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; - } + SHOW_FUNC_DEPRECATION_WARNING(); - 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; - } - } - } - } - } + return; } -//_____________________________________________________________________________ -/* - * _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 +void OpenSim::GeometryPath::setLength(const SimTK::State&, double) const { - const AbstractPathPoint* pt1 = path.get(wr.startPoint); - const AbstractPathPoint* pt2 = path.get(wr.endPoint); - - double straight_length = pt1->calcDistanceBetween(s, *pt2); + SHOW_FUNC_DEPRECATION_WARNING(); - 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 + return; } -//_____________________________________________________________________________ -/* - * 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 +void OpenSim::GeometryPath::setLengtheningSpeed(const SimTK::State&, double) 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); - } - } + SHOW_FUNC_DEPRECATION_WARNING(); - setLength(s,length); - return( length ); + return; } -//_____________________________________________________________________________ -/* - * Compute the path's moment arms for specified coordinate. - * - * @param aCoord, the coordinate - */ -double GeometryPath:: -computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const +void OpenSim::GeometryPath::getPointForceDirections( + const SimTK::State&, + OpenSim::Array*) const { - if (!_maSolver) - const_cast(this)->_maSolver.reset(new MomentArmSolver(*_model)); + SHOW_FUNC_DEPRECATION_WARNING(); - return _maSolver->solve(s, aCoord, *this); + return; } -//_____________________________________________________________________________ -// Override default implementation by object to intercept and fix the XML node -// underneath the model to match current version. -void GeometryPath::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber) +void OpenSim::GeometryPath::updateGeometry(const SimTK::State&) const { - if (versionNumber < XMLDocument::getLatestVersion()) { - if (versionNumber < 30516) { - // Create Appearance node under GeometryPath - SimTK::Xml::Element appearanceElement("Appearance"); - aNode.appendNode(appearanceElement); - SimTK::Xml::element_iterator visObjectIter = aNode.element_begin("VisibleObject"); - if (visObjectIter != aNode.element_end()) { - SimTK::Xml::element_iterator oldPrefIter = visObjectIter->element_begin("display_preference"); - // old display_preference was used only for hide/show other options unsupported - if (oldPrefIter != visObjectIter->element_end()) { - int oldPrefAsInt = 4; - oldPrefIter->getValueAs(oldPrefAsInt); - if (oldPrefAsInt == 0) { // Hidden => Appearance/Visible - SimTK::Xml::Element visibleElement("visible"); - visibleElement.setValueAs(false); - appearanceElement.insertNodeAfter(appearanceElement.element_end(), visibleElement); - } - } - } - // If default_color existed, copy it to Appearance/color - SimTK::Xml::element_iterator defaultColorIter = aNode.element_begin("default_color"); - if (defaultColorIter != aNode.element_end()) { - // Move default_color to Appearance/color - SimTK::Xml::Element colorElement("color"); - const SimTK::String& colorAsString = defaultColorIter->getValue(); - colorElement.setValue(colorAsString); - appearanceElement.appendNode(colorElement); - } - } - } - // Call base class now assuming aNode has been corrected for current version - Super::updateFromXMLNode(aNode, versionNumber); + SHOW_FUNC_DEPRECATION_WARNING(); + + return; } diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index 045efe5888..fdf746b5aa 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,11 @@ * 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 @@ -41,29 +39,45 @@ namespace OpenSim { +class AbstractPathPoint; class Coordinate; +class PhysicalFrame; class PointForceDirection; 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_OUTPUT(length, double, getLength, SimTK::Stage::Position); - // - OpenSim_DECLARE_OUTPUT(lengthening_speed, double, getLengtheningSpeed, - SimTK::Stage::Velocity); + 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); //============================================================================= // DATA @@ -72,184 +86,285 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); OpenSim_DECLARE_UNNAMED_PROPERTY(Appearance, "Default appearance attributes for this GeometryPath"); + class Impl; private: - 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"); - - // used for scaling tendon and fiber lengths - double _preScaleLength; - - // Solver used to compute moment-arms. The GeometryPath owns this object, - // but we cannot simply use a unique_ptr because we want the pointer to be - // cleared on copy. - SimTK::ResetOnCopy > _maSolver; + SimTK::ClonePtr _impl; - mutable CacheVariable _lengthCV; - mutable CacheVariable _speedCV; - mutable CacheVariable> _currentPathCV; - mutable CacheVariable _colorCV; - //============================================================================= // 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. **/ - const SimTK::Vec3& getDefaultColor() const { return get_Appearance().get_color(); } - - /** %Set the value of the color cache variable owned by this %GeometryPath - object, in the cache of the given state. The value of this variable is used - as the color when the path is drawn, which occurs with the state realized - to Stage::Dynamics. So you must call this method during realizeDynamics() or - earlier in order for it to have any effect. **/ - void setColor(const SimTK::State& s, const SimTK::Vec3& color) const; - - /** Get the current value of the color cache entry owned by this - %GeometryPath object in the given state. You can access this value any time - after the state is initialized, at which point it will have been set to - the default color value specified in a call to setDefaultColor() earlier, - or it will have the default color value chosen by %GeometryPath. - @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 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; - - /** add in the equivalent body and generalized forces to be applied to the - multibody system resulting from a tension along the GeometryPath - @param state state used to evaluate forces - @param[in] tension scalar (double) of the applied (+ve) tensile force - @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; - - - //-------------------------------------------------------------------------- - // COMPUTATIONS - //-------------------------------------------------------------------------- - virtual double computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const; - - //-------------------------------------------------------------------------- - // SCALING - //-------------------------------------------------------------------------- - - /** Calculate the path length in the current body position and store it for - use after the Model has been scaled. */ - void extendPreScale(const SimTK::State& s, - const ScaleSet& scaleSet) override; - - /** Recalculate the path after the Model has been scaled. */ - void extendPostScale(const SimTK::State& s, - const ScaleSet& scaleSet) override; - - //-------------------------------------------------------------------------- - // Visualization Support - //-------------------------------------------------------------------------- - // Update the geometry attached to the path (location of path points and - // connecting segments all in global/inertial frame) + GeometryPath(const GeometryPath&); + ~GeometryPath() noexcept; + + GeometryPath& operator=(const GeometryPath&); + + + // INTERFACE METHODS + // + // Concrete implementations of `GeometryPath` *must* provide these. + + /** + * Get the current color of the path. + * + * This is the runtime, potentially state-dependent, color of the path. It + * is the color used to display the path in that state (e.g. for UI rendering). + * + * This color value is typically initialized with the default color (see: + * `getDefaultColor`), but the color can change between simulation states + * because downstream code (e.g. muscles) might call `setColor` to implement + * state-dependent path coloring. + */ + virtual SimTK::Vec3 getColor(const SimTK::State& s) const = 0; + + /** + * Set the current color of the path. + * + * Internally, sets the current color value of the path for the provided state + * (e.g. using cache variables). + * + * The value of this variable is used as the color when the path is drawn, which + * occurs with the state realized to Stage::Dynamics. Therefore, you must call + * this method during realizeDynamics() or earlier in order for it to have any + * effect. + */ + virtual void setColor(const SimTK::State& s, const SimTK::Vec3& color) const = 0; + + /** + * Get the current length of the path. + * + * Internally, this may use a variety of methods to figure out how long the path + * is, such as using spline-fits, or computing the distance between points in + * space. It is up to concrete implementations (e.g. `PointBasedPath`) to provide + * a relevant implementation. + */ + virtual double getLength(const SimTK::State& s) const = 0; + + /** + * Get the lengthening speed of the path. + * + * Internally, this may use a variety of methods to figure out the lengthening + * speed. It might use the finite difference between two lengths, or an analytic + * solution, or always return `0.0`. It is up to concrete implementations (e.g. + * `PointBasedPath`) to provide a relevant implementation. + */ + virtual double getLengtheningSpeed(const SimTK::State& s) const = 0; + + /** + * Add in the equivalent body and generalized forces to be applied to the + * multibody system resulting from a tension along the GeometryPath. + * + * @param state state used to evaluate forces + * @param[in] tension scalar (double) of the applied (+ve) tensile force + * @param[in,out] bodyForces Vector of SpatialVec's (torque, force) on bodies + * @param[in,out] mobilityForces Vector of generalized forces, one per mobility + */ + virtual void addInEquivalentForces(const SimTK::State& state, + double tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const = 0; + + /** + * Returns the moment arm of the path in the given state with respect to + * the specified coordinate. + */ + virtual double computeMomentArm(const SimTK::State& s, + const Coordinate& aCoord) const = 0; + + + // DEFAULTED METHODS + // + // These are non-deprecated methods that GeometryPath provides a default + // implementation of. + + /** + * Get the default color of the path. + * + * Returns the color that will be used to initialize the color cache + * at the next extendAddToSystem() call. Use `getColor` to retrieve the + * (potentially different) color that will be used to draw the path. + */ + const SimTK::Vec3& getDefaultColor() const; + + /** + * Set the default color of the path. + * + * Sets the internal, default, color value for the path. This is the color + * that's used when the simulation is initialized (specifically, during the + * `extendAddToSystem` call). + * + * This color is not necessarily the *current* color of the path. Other code + * in the system (e.g. muscle implementations) may change the runtime color + * with `setColor`. Use `getColor`, with a particular simulation state, to + * get the color of the path in that state. + */ + void setDefaultColor(const SimTK::Vec3& color); + + /** + * Get the current length of the path, *before* the last set of scaling operations + * were applied to it. + * + * Internally, the path stores the original length in a `double` during `extendPreScale`. + * Therefore, be *very* careful with this method, because the recorded length is dependent + * on the length as computed during `extendPreScale`, which may have been called with a + * different state. + */ + double getPreScaleLength(const SimTK::State& s) const; + void setPreScaleLength(const SimTK::State& s, double preScaleLength); + + + // DEPRECATED METHODS + // + // These are here for backwards compatability. Most of these methods are + // here because `GeometryPath` used to always be a `PointBasedPath`. + // In later versions of OpenSim, `GeometryPath` was generalized to mean + // "any path with a length and lengthening speed" to support features + // like spline-based muscle paths. + // + // The base implementation of these methods provides a "stub" implementation + // that is guaranteed to at least be crash-free and return valid points, but + // not actually affect the path. Downstream implementations (e.g. an actual + // `PointBasedPath`) can override these to provide a "real" implementation. + + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual const PathPointSet& getPathPointSet() const; + + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual PathPointSet& updPathPointSet(); + + /** + * 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. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual AbstractPathPoint* addPathPoint( + const SimTK::State& s, + int index, + const PhysicalFrame& frame); + + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual AbstractPathPoint* appendNewPathPoint( + const std::string& proposedName, + const PhysicalFrame& frame, + const SimTK::Vec3& locationOnFrame); + + /** + * Returns true 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. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual bool canDeletePathPoint(int index); + + /** + * Delete a path point. + * + * @param aIndex The index of the point to delete. + * @return Whether or not the point was deleted. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual bool deletePathPoint(const SimTK::State& s, int index); + + /** + * 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. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual bool replacePathPoint( + const SimTK::State& s, + AbstractPathPoint* oldPathPoint, + AbstractPathPoint* newPathPoint); + + /** + * Get the current path of the path. + * + * @return The array of currently active path points. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual const Array& getCurrentPath(const SimTK::State& s) const; + + + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual const PathWrapSet& getWrapSet() const; + + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual PathWrapSet& updWrapSet(); + + /** + * Create a new wrap instance and add it to the set. + * + * @param aWrapObject The wrap object to use in the new wrap instance. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual void addPathWrap(WrapObject& aWrapObject); + + /** + * 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. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual void moveUpPathWrap(const SimTK::State& s, int index); + + /** + * 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. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual void moveDownPathWrap(const SimTK::State& s, int index); + + /** + * Delete a wrap instance. + * + * @param aIndex The index of the wrap instance to delete. + */ + DEPRECATED_14("Avoid using this method on the GeometryPath base class, because the path may not contain points (e.g. if it is function-based). Instead, test whether the path is a PointBasedPath (e.g. by downcasting with dynamic_cast or similar) and then use the same method on PointBasedPath (which will definitely work as intended)") + virtual void deletePathWrap(const SimTK::State& s, int index); + + + DEPRECATED_14("Avoid using GeometryPath::setLength(...): it shouldn't be possible to externally set the length of a (potentially, computed) path.") + virtual void setLength(const SimTK::State& s, double length) const; + + DEPRECATED_14("Avoid using GeometryPath::setLengtheningSpeed(...): it shouldn't be possible to externally set the lengthening speed of a (potentially, computed) path.") + virtual void setLengtheningSpeed(const SimTK::State& s, double speed) const; + + /** + * Appends PointForceDirections to the output parameter. + * + * These can be used to apply tension to bodies the points are connected to. + * + * CAUTION: the return pointers are heap allocated: you must delete them yourself! + */ + DEPRECATED_14("Avoid using GeometryPath::getPointForceDirections(...): prefer GeometryPath::addInEquivalentForces(...) instead.") + virtual void getPointForceDirections( + const SimTK::State& s, + OpenSim::Array* rPFDs) const; + + /** + * Proactively updates any decorative geometry attached to the path. + * + * This method is an advanced optimization method that attempts to + * proactively populate any internal datastructures that are used to + * generate path decorations. E.g. the location of path points, the + * connecting segments/cylinders of those path points, etc. + */ + DEPRECATED_14("Avoid using GeometryPath::updateGeometry(...): implementations of GeometryPath should handle any decoration updates, caching, etc. when the caller uses `GeometryPath::generateDecorations(...)`") virtual void updateGeometry(const SimTK::State& s) const; - -protected: - // ModelComponent interface. - void extendConnectToModel(Model& aModel) override; - 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: - - 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; - - 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; - -//============================================================================= -}; // END of class GeometryPath -//============================================================================= -//============================================================================= - -} // end of namespace OpenSim +}; +} #endif // OPENSIM_GEOMETRY_PATH_H_ diff --git a/OpenSim/Simulation/Model/Ligament.cpp b/OpenSim/Simulation/Model/Ligament.cpp index 942a90e426..5d14d4726b 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 "PointBasedPath.h" #include "PointForceDirection.h" #include @@ -63,7 +65,7 @@ Ligament::Ligament() void Ligament::constructProperties() { setAuthors("Peter Loan"); - constructProperty_GeometryPath(GeometryPath()); + constructProperty_GeometryPath(PointBasedPath{}); constructProperty_resting_length(0.0); constructProperty_pcsa_force(0.0); @@ -243,14 +245,6 @@ void Ligament::computeForce(const SimTK::State& s, SimTK::Vector(1, path.getLength(s)/restingLength))* pcsaForce; setCacheVariableValue(s, _tensionCV, force); - OpenSim::Array PFDs; - path.getPointForceDirections(s, &PFDs); - - for (int i=0; i < PFDs.getSize(); i++) { - applyForceToPoint(s, PFDs[i]->frame(), PFDs[i]->point(), - force*PFDs[i]->direction(), bodyForces); - } - for(int i=0; i < PFDs.getSize(); i++) - delete PFDs[i]; + path.addInEquivalentForces(s, force, bodyForces, generalizedForces); } 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/PathActuator.cpp b/OpenSim/Simulation/Model/PathActuator.cpp index 130965241c..0922e3d3b8 100644 --- a/OpenSim/Simulation/Model/PathActuator.cpp +++ b/OpenSim/Simulation/Model/PathActuator.cpp @@ -26,6 +26,8 @@ //============================================================================= #include "PathActuator.h" +#include + using namespace OpenSim; using namespace std; @@ -62,7 +64,7 @@ void PathActuator::setNull() */ void PathActuator::constructProperties() { - constructProperty_GeometryPath(GeometryPath()); + constructProperty_GeometryPath(PointBasedPath{}); constructProperty_optimal_force(1.0); } diff --git a/OpenSim/Simulation/Model/PathSpring.cpp b/OpenSim/Simulation/Model/PathSpring.cpp index 2ba09fac02..e4c38b1f10 100644 --- a/OpenSim/Simulation/Model/PathSpring.cpp +++ b/OpenSim/Simulation/Model/PathSpring.cpp @@ -25,7 +25,8 @@ // INCLUDES //============================================================================= #include "PathSpring.h" -#include "GeometryPath.h" + +#include #include "PointForceDirection.h" //============================================================================= @@ -64,7 +65,7 @@ PathSpring::PathSpring(const string& name, double restLength, void PathSpring::constructProperties() { setAuthors("Ajay Seth"); - constructProperty_GeometryPath(GeometryPath()); + constructProperty_GeometryPath(PointBasedPath{}); constructProperty_resting_length(SimTK::NaN); constructProperty_stiffness(SimTK::NaN); constructProperty_dissipation(SimTK::NaN); diff --git a/OpenSim/Simulation/Model/PointBasedPath.cpp b/OpenSim/Simulation/Model/PointBasedPath.cpp new file mode 100644 index 0000000000..7e4bfffa1e --- /dev/null +++ b/OpenSim/Simulation/Model/PointBasedPath.cpp @@ -0,0 +1,1065 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: PointBasedPath.cpp * + * -------------------------------------------------------------------------- * + * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * + * See http://opensim.stanford.edu and the NOTICE file for more information. * + * OpenSim is developed at Stanford University and supported by the US * + * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * + * through the Warrior Web program. * + * * + * Copyright (c) 2005-2017 Stanford University and the Authors * + * Author(s): Peter Loan, Ajay Seth * + * * + * 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 "PointBasedPath.h" + +#include "ConditionalPathPoint.h" +#include "MovingPathPoint.h" +#include "PointForceDirection.h" +#include +#include "Model.h" + +OpenSim::PointBasedPath::PointBasedPath() : GeometryPath{}, _maSolver{nullptr} +{ + setAuthors("Peter Loan"); + constructProperty_PathPointSet(PathPointSet()); + constructProperty_PathWrapSet(PathWrapSet()); +} + +const OpenSim::PathPointSet& OpenSim::PointBasedPath::getPathPointSet() const +{ + return get_PathPointSet(); +} + +OpenSim::PathPointSet& OpenSim::PointBasedPath::updPathPointSet() +{ + return upd_PathPointSet(); +} + +OpenSim::AbstractPathPoint* OpenSim::PointBasedPath::addPathPoint( + const SimTK::State& s, + int aIndex, + const PhysicalFrame& frame) +{ + PathPoint* newPoint = new PathPoint(); + newPoint->setParentFrame(frame); + SimTK::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; +} + +bool OpenSim::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; +} + +bool OpenSim::PointBasedPath::deletePathPoint(const SimTK::State& s, int aIndex) +{ + if (!canDeletePathPoint(aIndex)) { + 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; +} + +bool OpenSim::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; +} + +const OpenSim::Array& OpenSim::PointBasedPath::getCurrentPath( + const SimTK::State& s) const +{ + computePath(s); // compute checks if path needs to be recomputed + return getCacheVariableValue< Array >(s, _currentPathCV); +} + + +const OpenSim::PathWrapSet& OpenSim::PointBasedPath::getWrapSet() const +{ + return get_PathWrapSet(); +} + +OpenSim::PathWrapSet& OpenSim::PointBasedPath::updWrapSet() +{ + return upd_PathWrapSet(); +} + +void OpenSim::PointBasedPath::addPathWrap(WrapObject& aWrapObject) +{ + PathWrap* newWrap = new PathWrap(); + newWrap->setWrapObject(aWrapObject); + newWrap->setMethod(PathWrap::hybrid); + upd_PathWrapSet().adoptAndAppend(newWrap); + finalizeFromProperties(); +} + +void OpenSim::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); + } +} + +void OpenSim::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); + } +} + +void OpenSim::PointBasedPath::deletePathWrap(const SimTK::State& s, int aIndex) +{ + upd_PathWrapSet().remove(aIndex); +} + +SimTK::Vec3 OpenSim::PointBasedPath::getColor(const SimTK::State& s) const +{ + return getCacheVariableValue(s, _colorCV); +} + +void OpenSim::PointBasedPath::setColor(const SimTK::State& s, + const SimTK::Vec3& color) const +{ + setCacheVariableValue(s, _colorCV, color); +} + + +double OpenSim::PointBasedPath::getLength(const SimTK::State& s) const +{ + computePath(s); // compute checks if path needs to be recomputed + return getCacheVariableValue(s, _lengthCV); +} + +void OpenSim::PointBasedPath::setLength(const SimTK::State& s, double length) const +{ + setCacheVariableValue(s, _lengthCV, length); +} + + +double OpenSim::PointBasedPath::getLengtheningSpeed(const SimTK::State& s) const +{ + computeLengtheningSpeed(s); + return getCacheVariableValue(s, _speedCV); +} + +void OpenSim::PointBasedPath::setLengtheningSpeed(const SimTK::State& s, double speed) const +{ + setCacheVariableValue(s, _speedCV, speed); +} + +void OpenSim::PointBasedPath::getPointForceDirections( + const SimTK::State& s, + OpenSim::Array *rPFDs) const +{ + using SimTK::Vec3; + + 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); + } + } +} + +void OpenSim::PointBasedPath::addInEquivalentForces( + const SimTK::State& s, + double tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const +{ + using SimTK::Vec3; + + 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); + } + } + } +} + +double OpenSim::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); +} + + +void OpenSim::PointBasedPath::extendPreScale( + const SimTK::State& s, + const ScaleSet& scaleSet) +{ + Super::extendPreScale(s, scaleSet); + setPreScaleLength(s, getLength(s)); +} + +void OpenSim::PointBasedPath::extendPostScale( + const SimTK::State& s, + const ScaleSet& scaleSet) +{ + Super::extendPostScale(s, scaleSet); + computePath(s); +} + + +void OpenSim::PointBasedPath::updateGeometry(const SimTK::State& s) const +{ + // Check if the current path needs to recomputed. + computePath(s); +} + + +void OpenSim::PointBasedPath::extendFinalizeFromProperties() +{ + Super::extendFinalizeFromProperties(); + + for (int i = 0; i < get_PathWrapSet().getSize(); ++i) { + if (upd_PathWrapSet()[i].getName().empty()) { + std::stringstream label; + label << "pathwrap_" << i; + upd_PathWrapSet()[i].setName(label.str()); + } + } +} + +void OpenSim::PointBasedPath::extendConnectToModel(Model& aModel) +{ + Super::extendConnectToModel(aModel); + + OPENSIM_THROW_IF_FRMOBJ(get_PathPointSet().getSize() < 2, + InvalidPropertyValue, + getProperty_PathPointSet().getName(), + "A valid path must be connected to a model by at least two PathPoints.") + + // Name the path points based on the current path + // (i.e., the set of currently active points is numbered + // 1, 2, 3, ...). + namePathPoints(0); +} + +void OpenSim::PointBasedPath::extendAddToSystem(SimTK::MultibodySystem& system) const +{ + Super::extendAddToSystem(system); + + // Allocate cache entries to save the current length and speed(=d/dt length) + // of the path in the cache. Length depends only on q's so will be valid + // after Position stage, speed requires u's also so valid at Velocity stage. + this->_lengthCV = addCacheVariable("length", 0.0, SimTK::Stage::Position); + this->_speedCV = addCacheVariable("speed", 0.0, SimTK::Stage::Velocity); + + // Cache the set of points currently defining this path. + this->_currentPathCV = addCacheVariable("current_path", Array{}, SimTK::Stage::Position); + + // We consider this cache entry valid any time after it has been created + // and first marked valid, and we won't ever invalidate it. + this->_colorCV = addCacheVariable("color", get_Appearance().get_color(), SimTK::Stage::Topology); +} + +void OpenSim::PointBasedPath::extendInitStateFromProperties(SimTK::State& s) const +{ + Super::extendInitStateFromProperties(s); + 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 OpenSim::PointBasedPath::generateDecorations( + bool fixed, + const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& appendToThis) const +{ + if (fixed) { + return; // there is no fixed geometry to generate here + } + + const Array& pathPoints = getCurrentPath(state); + + assert(pathPoints.size() > 1); + + const AbstractPathPoint* lastPoint = pathPoints[0]; + SimTK::MobilizedBodyIndex mbix(0); + + SimTK::Vec3 lastPos = lastPoint->getLocationInGround(state); + if (hints.get_show_path_points()) + SimTK::DefaultGeometry::drawPathPoint(mbix, lastPos, getColor(state), appendToThis); + + SimTK::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 SimTK::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()) + SimTK::DefaultGeometry::drawPathPoint(mbix, pos, getColor(state), + appendToThis); + // Line segments will be in ground frame + appendToThis.push_back(SimTK::DecorativeLine(lastPos, pos) + .setLineThickness(4) + .setColor(getColor(state)).setBodyId(0).setIndexOnBody(i)); + lastPos = pos; + } + } +} + + +// private methods + +void OpenSim::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); +} + +void OpenSim::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); +} + +void OpenSim::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; + } + } + } + } + } +} + +/* + * _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 OpenSim::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 OpenSim::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 ); +} + +/* + * 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 OpenSim::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 OpenSim::PointBasedPath::placeNewPathPoint( + const SimTK::State& s, + SimTK::Vec3& aOffset, + int aIndex, + const PhysicalFrame& frame) +{ + using SimTK::Vec3; + + // 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? + } +} + +void OpenSim::PointBasedPath::updateFromXMLNode( + SimTK::Xml::Element& aNode, + int versionNumber) +{ + if (versionNumber < XMLDocument::getLatestVersion()) { + if (versionNumber < 30516) { + // Create Appearance node under GeometryPath + SimTK::Xml::Element appearanceElement("Appearance"); + aNode.appendNode(appearanceElement); + SimTK::Xml::element_iterator visObjectIter = aNode.element_begin("VisibleObject"); + if (visObjectIter != aNode.element_end()) { + SimTK::Xml::element_iterator oldPrefIter = visObjectIter->element_begin("display_preference"); + // old display_preference was used only for hide/show other options unsupported + if (oldPrefIter != visObjectIter->element_end()) { + int oldPrefAsInt = 4; + oldPrefIter->getValueAs(oldPrefAsInt); + if (oldPrefAsInt == 0) { // Hidden => Appearance/Visible + SimTK::Xml::Element visibleElement("visible"); + visibleElement.setValueAs(false); + appearanceElement.insertNodeAfter(appearanceElement.element_end(), visibleElement); + } + } + } + // If default_color existed, copy it to Appearance/color + SimTK::Xml::element_iterator defaultColorIter = aNode.element_begin("default_color"); + if (defaultColorIter != aNode.element_end()) { + // Move default_color to Appearance/color + SimTK::Xml::Element colorElement("color"); + const SimTK::String& colorAsString = defaultColorIter->getValue(); + colorElement.setValue(colorAsString); + appearanceElement.appendNode(colorElement); + } + } + } + + // Call base class now assuming aNode has been corrected for current version + Super::updateFromXMLNode(aNode, versionNumber); +} diff --git a/OpenSim/Simulation/Model/PointBasedPath.h b/OpenSim/Simulation/Model/PointBasedPath.h new file mode 100644 index 0000000000..187cc3195d --- /dev/null +++ b/OpenSim/Simulation/Model/PointBasedPath.h @@ -0,0 +1,162 @@ +#ifndef OPENSIM_POINTBASED_PATH_H_ +#define OPENSIM_POINTBASED_PATH_H_ + +/* -------------------------------------------------------------------------- * + * OpenSim: PointBasedPath.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 + +#ifdef SWIG + #ifdef OSIMSIMULATION_API + #undef OSIMSIMULATION_API + #define OSIMSIMULATION_API + #endif +#endif + +namespace OpenSim { +class OSIMSIMULATION_API PointBasedPath final : public GeometryPath { + OpenSim_DECLARE_CONCRETE_OBJECT(PointBasedPath, GeometryPath); + +private: + 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"); + + // Solver used to compute moment-arms. The GeometryPath owns this object, + // but we cannot simply use a unique_ptr because we want the pointer to be + // cleared on copy. + SimTK::ResetOnCopy > _maSolver; + + mutable CacheVariable _lengthCV; + mutable CacheVariable _speedCV; + mutable CacheVariable> _currentPathCV; + mutable CacheVariable _colorCV; + +public: + PointBasedPath(); + + const PathPointSet& getPathPointSet() const override; + PathPointSet& updPathPointSet() 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; + const Array& getCurrentPath(const SimTK::State& s) const override; + + + const PathWrapSet& getWrapSet() const override; + PathWrapSet& updWrapSet() override; + void addPathWrap(WrapObject& aWrapObject) override; + void moveUpPathWrap(const SimTK::State& s, int index) override; + void moveDownPathWrap(const SimTK::State& s, int index) override; + void deletePathWrap(const SimTK::State& s, int index) override; + + SimTK::Vec3 getColor(const SimTK::State& s) const override; + void setColor(const SimTK::State& s, const SimTK::Vec3& color) const 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; + + void getPointForceDirections( + const SimTK::State& s, + OpenSim::Array* rPFDs) const override; + + void addInEquivalentForces(const SimTK::State& state, + double tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const override; + + double computeMomentArm(const SimTK::State& s, + const Coordinate& aCoord) const override; + void updateGeometry(const SimTK::State& s) const override; + + /** + * Calculate the path length in the current body position and store it for + * use after the Model has been scaled. + */ + void extendPreScale(const SimTK::State& s, + const ScaleSet& scaleSet) override; + + /** + * Recalculate the path after the Model has been scaled. + */ + void extendPostScale(const SimTK::State& s, + const ScaleSet& scaleSet) override; + +protected: + void extendFinalizeFromProperties() override; + void extendConnectToModel(Model& aModel) override; + void extendAddToSystem(SimTK::MultibodySystem& system) const override; + void extendInitStateFromProperties(SimTK::State& s) const override; + + void generateDecorations(bool fixed, + const ModelDisplayHints& hints, + const SimTK::State& state, + SimTK::Array_& appendToThis) const override; + +private: + 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; + + + void namePathPoints(int aStartingIndex); + void placeNewPathPoint(const SimTK::State& s, + SimTK::Vec3& aOffset, + int index, + const PhysicalFrame& frame); + + // Override of the default implementation to account for versioning. + void updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumber = -1) override; +}; +} + +#endif diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 38b766acca..73006f3313 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -59,7 +59,8 @@ #include "Model/PathPointSet.h" #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" -#include "Model/GeometryPath.h" +#include "Model/PointBasedPath.h" +#include "Model/FunctionBasedPath.h" #include "Model/PrescribedForce.h" #include "Model/ExternalForce.h" #include "Model/PointToPointSpring.h" @@ -188,7 +189,8 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( LineGeometry()); Object::registerType( FrameGeometry()); Object::registerType( Arrow()); - Object::registerType( GeometryPath()); + Object::registerType( PointBasedPath()); + Object::registerType( FunctionBasedPath()); Object::registerType( ControlSet() ); Object::registerType( ControlConstant() ); @@ -291,6 +293,7 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() // Associate an instance with old name to help deserialization. // This has to be done after the new Type is registered. Object::renameType("ActuatorSet", "ForceSet"); + Object::renameType("GeometryPath", "PointBasedPath"); Object::renameType("MuscleWrap", "PathWrap"); Object::renameType("MuscleWrapSet", "PathWrapSet"); Object::renameType("MusclePoint", "PathPoint"); diff --git a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp new file mode 100644 index 0000000000..5ac4db6f77 --- /dev/null +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -0,0 +1,1492 @@ +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +// test support code +// +// enables the OSIM_TEST macro, and other nice-to-haves +namespace { + struct Test { char const* suiteName; char const* name; void(*testFunc)(void); }; + + std::ostream& operator<<(std::ostream& o, Test const& t) + { + return o << t.suiteName << '/' << t.name; + } + + static std::vector& GetTestList() + { + static std::vector g_TestList; + return g_TestList; + } + + static void RegisterTest(char const* suiteName, char const* name, void(*testFunc)(void)) + { + GetTestList().push_back(Test{suiteName, name, testFunc}); + } + + #define OSIM_TEST( SuiteName , TestName ) \ + static void Test_ ## SuiteName ## TestName(); \ + static bool g_Add_ ## SuiteName ## TestName ## _ToTestList = [](){ RegisterTest(#SuiteName, #TestName, Test_ ## SuiteName ## TestName); return true; }(); \ + static void Test_ ## SuiteName ## TestName() + + static int RunAllTests() + { + std::cerr << "[ ====== ] Running " << GetTestList().size() << " tests\n"; + + int numTests = 0; + int numFailures = 0; + auto timerAllTestsStart = std::chrono::high_resolution_clock::now(); + for (Test const& t : GetTestList()) { + ++numTests; + std::cerr << "[ RUN ] " << t << '\n'; + auto timerTestStart = std::chrono::high_resolution_clock::now(); + try { + t.testFunc(); + auto timerTestEnd = std::chrono::high_resolution_clock::now(); + auto timerMillis = std::chrono::duration_cast(timerTestEnd - timerTestStart); + std::cerr << "[ OK ] " << t << " (" << timerMillis.count() << " ms)\n"; + } catch (std::exception const& ex) { + auto timerTestEnd = std::chrono::high_resolution_clock::now(); + auto timerMillis = std::chrono::duration_cast(timerTestEnd - timerTestStart); + ++numFailures; + std::cerr << "[ FAILED ] " << t << " (" << timerMillis.count() << " ms)\n"; + std::cerr << "Exception message: " << ex.what() << '\n'; + } + } + auto timerAllTestsEnd = std::chrono::high_resolution_clock::now(); + std::chrono::milliseconds allTestsMillis = std::chrono::duration_cast(timerAllTestsEnd - timerAllTestsStart); + + std::cerr << "[ ====== ] " << numTests << " tests ran (" << allTestsMillis.count() << " ms total)\n"; + + if (numFailures == 0) { + std::cerr << "All tests (" << numTests << ") passed\n"; + return 0; + } else { + std::cerr << "There were " << numFailures << " failed tests\n"; + return 1; + } + } + + static double GenerateDouble() + { + static std::default_random_engine prng{std::random_device{}()}; + return std::uniform_real_distribution{}(prng); + } + + static SimTK::Vec3 GenerateRandomVector() + { + return SimTK::Vec3{GenerateDouble(), GenerateDouble(), GenerateDouble()}; + } + + static std::vector GenerateNPrefixedStrings(int n, std::string prefix = "str") + { + std::vector rv; + for (int i = 0; i < n; ++i) { + std::stringstream ss; + ss << prefix << i; + rv.push_back(ss.str()); + } + return rv; + } +} + +// FunctionBasedPath test support code +// +// enables mocking functions, etc. +namespace { + template + std::vector ToStdVector(const SimTK::Array_& stkArray) + { + std::vector rv; + rv.reserve(stkArray.size()); + for (const T& v : stkArray) { + rv.push_back(v); + } + return rv; + } + + template + std::vector ToStdVector(const SimTK::Vector_& stkVec) + { + std::vector rv; + rv.reserve(stkVec.size()); + for (int i = 0; i < stkVec.size(); ++i) { + rv.push_back(stkVec[i]); + } + return rv; + } + + struct SharedFunctionData final { + double value = 0.0; + double derivative = 0.0; + int argumentSize = 0; + int maxDerivativeOrder = 1; // these functions must be differentiable at least once + std::vector lastCalcValueArg = {}; + std::vector lastCalcDerivativeIntArg = {}; + std::vector lastCalcDerivativeValArgs = {}; + + int numTimesCalcValueCalled = 0; + int numTimesCalcDerivativeCalled = 0; + int numTimesGetArgumentSizeCalled = 0; + int numTimesGetMaxDerivativeOrderCalled = 0; + }; + + class MockSimTKFunction final : public SimTK::Function { + public: + mutable std::shared_ptr m_SharedData; + + MockSimTKFunction(std::shared_ptr sharedData) : + m_SharedData{sharedData} + { + } + + double calcValue(const SimTK::Vector& args) const override + { + ++m_SharedData->numTimesCalcValueCalled; + m_SharedData->lastCalcValueArg = ToStdVector(args); + return m_SharedData->value; + } + + double calcDerivative(const SimTK::Array_& intArg, const SimTK::Vector& valArgs) const override + { + ++m_SharedData->numTimesCalcDerivativeCalled; + m_SharedData->lastCalcDerivativeIntArg = ToStdVector(intArg); + m_SharedData->lastCalcDerivativeValArgs = ToStdVector(valArgs); + return m_SharedData->derivative; + } + + int getArgumentSize() const override + { + ++m_SharedData->numTimesGetArgumentSizeCalled; + return m_SharedData->argumentSize; + } + + int getMaxDerivativeOrder() const override + { + ++m_SharedData->numTimesGetMaxDerivativeOrderCalled; + return m_SharedData->maxDerivativeOrder; + } + + SimTK::Function* clone() const override + { + return new MockSimTKFunction{*this}; + } + }; + + // used to test that the FunctionBasedPath is forwarding things correctly + class MockPathFunction : public OpenSim::Function { + OpenSim_DECLARE_CONCRETE_OBJECT(MockPathFunction, OpenSim::Function); + + public: + mutable std::shared_ptr m_SharedData = std::make_shared(); + + double calcValue(const SimTK::Vector& args) const override + { + ++m_SharedData->numTimesCalcValueCalled; + m_SharedData->lastCalcValueArg = ToStdVector(args); + return m_SharedData->value; + } + + double calcDerivative(const std::vector& intArgs, const SimTK::Vector& valArgs) const override + { + ++m_SharedData->numTimesCalcDerivativeCalled; + m_SharedData->lastCalcDerivativeIntArg = intArgs; + m_SharedData->lastCalcDerivativeValArgs = ToStdVector(valArgs); + return m_SharedData->derivative; + } + + int getArgumentSize() const override + { + ++m_SharedData->numTimesGetArgumentSizeCalled; + return m_SharedData->argumentSize; + } + + int getMaxDerivativeOrder() const override + { + ++m_SharedData->numTimesGetMaxDerivativeOrderCalled; + return m_SharedData->maxDerivativeOrder; + } + + SimTK::Function* createSimTKFunction() const override + { + return new MockSimTKFunction{m_SharedData}; + } + }; + + // helper struct for generating a model with N coordinates + struct ModelWithNCoordinates { + OpenSim::Model model; + std::vector coordinateAbsPaths; + }; + + ModelWithNCoordinates GenerateModelWithNCoordinates(int n) + { + ModelWithNCoordinates rv; + + for (int i = 0; i < n; ++i) { + // create a dummy body the joint is joining to ground + OpenSim::Body* body = new OpenSim::Body{}; + body->setName(std::string{"body_"} + std::to_string(i)); + body->setMass(1.0); + rv.model.addBody(body); + + // create the joint and connect it to the body + ground + OpenSim::PinJoint* joint = new OpenSim::PinJoint{}; + joint->setName(std::string{"pinjoint_"} + std::to_string(i)); + joint->connectSocket_parent_frame(rv.model.getGround()); + joint->connectSocket_child_frame(*body); + rv.model.addComponent(joint); + + // save the coordinate's path (used by the FBP API) + rv.coordinateAbsPaths.push_back(joint->getCoordinate().getAbsolutePathString()); + } + + return rv; + } +} + + +// construction tests + +OSIM_TEST(FunctionBasedPath, CanBeDefaultConstructedWithoutThrowing) +{ + OpenSim::FunctionBasedPath fbp; // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, CanBeCopyConstructedWithoutThrowing) +{ + OpenSim::FunctionBasedPath fbp1; + OpenSim::FunctionBasedPath fbp2{fbp1}; // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, CanBeMoveConstructedWithoutThrowing) +{ + OpenSim::FunctionBasedPath fbp1; + OpenSim::FunctionBasedPath fbp2{std::move(fbp1)}; // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, CanBeConstructedWithA0ArgPathFunctionAndEmptyCoordinateListWithoutThrowing) +{ + MockPathFunction pathFn; + std::vector coords; + OpenSim::FunctionBasedPath fbp{pathFn, coords}; // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, CanBeConstructedWithAnNArityFunctionAndNCoordinates) +{ + // function-based paths have to support being parameterized with a varying + // number of coordinates - not just 0, 1, or 2 (etc.) + + for (int i = 0; i < 10; ++i) { + MockPathFunction fn; + fn.m_SharedData->argumentSize = i; + std::vector coords = GenerateNPrefixedStrings(i); + + OpenSim::FunctionBasedPath fbp{fn, coords}; // shouldn't throw + } +} + +OSIM_TEST(FunctionBasedPath, ConstructingWithDifferentArityAndNumberOfCoordinatesThrows) +{ + // the arity (number of arguments) the function accepts should be equal to the + // number of coordinates the caller specifies + // + // this is because, at simulation time, the FunctionBasedPath pumps the coordinate + // values into the function as arguments. Effectively, the generic function is + // parameterized by coordinate values. + + MockPathFunction fn; + fn.m_SharedData->argumentSize = 3; + std::vector coords = { "onlyone" }; + + ASSERT_THROW(OpenSim::Exception, OpenSim::FunctionBasedPath fbp(fn, coords)); +} + +OSIM_TEST(FunctionBasedPath, ConstructingWithAFunctionThatHasNoDerivativeThrows) +{ + // the provided function must be differentiable + // + // this is because the function needs to provide 0-order results (i.e. the + // length of the path) and 1st-order results (i.e. the lengthening speed of + // the path) + + MockPathFunction fn; + fn.m_SharedData->maxDerivativeOrder = 0; // non-differentiable function + + ASSERT_THROW(OpenSim::Exception, OpenSim::FunctionBasedPath fbp(fn, {})); +} + +OSIM_TEST(FunctionBasedPath, ConstructingWithAnEmptyCoordNameThrows) +{ + // the provided coordinate names should be non-empty + // + // this is a sanity-check by the implementation. It's entirely feasible + // that some external process (e.g. a file reader) pumps a blank line + // or whitespace string into this object's constructor - we try to + // aggressively find + report it early, rather than during some later + // runtime lookup + + MockPathFunction fn; + fn.m_SharedData->argumentSize = 1; + + ASSERT_THROW(OpenSim::Exception, OpenSim::FunctionBasedPath fbp(fn, {""})); +} + +OSIM_TEST(FunctionBasedPath, WhenConstructedWithPathFunctionUsesTheFunctionInLengthEvaluation) +{ + // this is partially an implementation detail - you can remove this test if you need + // + // the test is providing a 0-arity, 0-coordinate function to the path, but this + // test is ensuring that this (to be fair, bizzare) edge-case still ends up + // calling `getLength` with 0 args and ultimately returns some value + // + // see later API tests for how `getLength` etc. *should* be used: this is + // mostly just a sanity check on the "construct with a function" pattern that + // the test suite uses (prod uses are likely to just set property values on + // loading an OSIM file or something) + + OpenSim::Model model; + + MockPathFunction pathFn; + std::vector coords; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn, coords}; + + model.addComponent(fbp); + + SimTK_TEST(pathFn.m_SharedData->numTimesCalcValueCalled == 0); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(pathFn.m_SharedData->numTimesCalcValueCalled == 0); + + SimTK_TEST(fbp->getLength(s) == pathFn.m_SharedData->value); + + SimTK_TEST(pathFn.m_SharedData->numTimesCalcValueCalled == 1); +} + + +// assignment tests + +OSIM_TEST(FunctionBasedPath, CanBeCopyAssignedWithoutThrowing) +{ + OpenSim::FunctionBasedPath fbp1; + OpenSim::FunctionBasedPath fbp2; + fbp2 = fbp1; +} + +OSIM_TEST(FunctionBasedPath, CanBeMoveAssignedWithoutThrowing) +{ + OpenSim::FunctionBasedPath fbp1; + OpenSim::FunctionBasedPath fbp2; + fbp2 = std::move(fbp1); +} + + +// api tests + +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithoutSettingFunctionThrows) +{ + // calling finalizeFromProperties implies the FBP's properties are correct + // + // it is incorrect to leave the Function property not-set + + OpenSim::FunctionBasedPath fbp; + + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); +} + +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithNonDifferentiableFunctionThrows) +{ + // calling finalizeFromProperties implies the FBP's properties are correct + // + // it is incorrect to set the Function property to a function that cannot + // be differentiated (an API requirement) + + OpenSim::FunctionBasedPath fbp; + + MockPathFunction fn; + fn.m_SharedData->maxDerivativeOrder = 0; // non-differentiable + + auto& prop = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + prop.setValue(fn); + + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); +} + +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithInvalidFunctionArityThrows) +{ + // calling finalizeFromProperties implies the FBP's properties are correct + // + // it is incorrect to set the Function property to a function that has a + // different arity (number of arguments) from the number of coordinates + // defined in the coordinates property + + OpenSim::FunctionBasedPath fbp; + + MockPathFunction fn; + fn.m_SharedData->argumentSize = 10; + + auto& prop = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + prop.setValue(fn); + + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); +} + +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithEmptyCoordinateStringThrows) +{ + // calling finalizeFromProperties implies the FBP's properties are correct + // + // it is incorrect to provide an empty coordinate string as a property (every + // string should be a nonempty absolute path to a coordinate) + + OpenSim::FunctionBasedPath fbp; + + MockPathFunction fn; + fn.m_SharedData->argumentSize = 1; + auto& funcProp = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + funcProp.setValue(fn); + + auto& coordProp = dynamic_cast&>(fbp.updPropertyByName("Coordinates")); + coordProp.adoptAndAppendValue(new std::string{""}); + + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); +} + +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithCorrectPropertiesDoesNotThrow) +{ + // this is a throwaway test that ensures the sane-case doesn't throw + + OpenSim::FunctionBasedPath fbp; + + MockPathFunction fn; + fn.m_SharedData->argumentSize = 0; + auto& funcProp = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + funcProp.setValue(fn); + + fbp.finalizeFromProperties(); // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, FinalizeConnectionsWithCorrectPropertiesDoesNotThrow) +{ + // calling `finalizeConnections` should be fine if every coordinate path + // provided to the FBP constructor is actually a real coordinate + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); + + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, m.coordinateAbsPaths}; + m.model.addComponent(fbp); + + m.model.finalizeFromProperties(); + m.model.finalizeConnections(); +} + +OSIM_TEST(FunctionBasedPath, FinalizeConnectionsWithIncorrectCoordinatePathThrows) +{ + // calling `finalizeConnections` should throw with a (hopefully) handy error + // message at conneciton-time if an FBP contains an invalid coordinate path + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); + + auto paths = m.coordinateAbsPaths; + paths[2] = "not-a-real-coordinate-path"; + + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, paths}; + m.model.addComponent(fbp); + + m.model.finalizeFromProperties(); + ASSERT_THROW(OpenSim::Exception, m.model.finalizeConnections()); +} + +OSIM_TEST(FunctionBasedPath, CanGetColorWithoutThrowing) +{ + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{MockPathFunction{}, {}}; + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + fbp->getColor(s); +} + +OSIM_TEST(FunctionBasedPath, GetColorReturnsDefaultColorIfSetColorIsNotCalled) +{ + SimTK::Vec3 randomColor = GenerateRandomVector(); + + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{MockPathFunction{}, {}}; + fbp->setDefaultColor(randomColor); + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(fbp->getColor(s) == randomColor); +} + +OSIM_TEST(FunctionBasedPath, SetColorSetsTheColorInTheState) +{ + SimTK::Vec3 randomColor = GenerateRandomVector(); + + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{MockPathFunction{}, {}}; + fbp->setDefaultColor(randomColor); + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + fbp->setColor(s, randomColor); + + SimTK_TEST(fbp->getColor(s) == randomColor); +} + +OSIM_TEST(FunctionBasedPath, GetLengthUsesUnderlyingFunctionCalcValue) +{ + // the FunctionBasedPath ultimately gets its result values from the underlying + // Function object, rather than generating them itself + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); + OpenSim::Coordinate const& coord = m.model.getComponent(m.coordinateAbsPaths.at(0)); + + MockPathFunction fn; + fn.m_SharedData->argumentSize = 1; + std::vector coordPaths = {coord.getAbsolutePathString()}; + + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, coordPaths}; + m.model.addComponent(fbp); + + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 0); + + SimTK::State& s = m.model.initSystem(); + + SimTK_TEST(fbp->getLength(s) == fn.m_SharedData->value); + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 1); + SimTK_TEST(fn.m_SharedData->lastCalcValueArg.size() == 1); + SimTK_TEST(fn.m_SharedData->lastCalcValueArg == std::vector{coord.getValue(s)}); +} + +OSIM_TEST(FunctionBasedPath, GetLengthIsCached) +{ + // FunctionBasedPath::getLength should be "cached" depending on the simulation + // stage. + // + // this is an implementation detail that's good for implementors to know. Remove + // this test if it's a bad assumption. + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); + + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, m.coordinateAbsPaths}; + m.model.addComponent(fbp); + + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 0); + + SimTK::State& s = m.model.initSystem(); + m.model.realizePosition(s); + + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 0); + SimTK_TEST(fbp->getLength(s) == fn.m_SharedData->value); + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 1); + fbp->getLength(s); // should cache + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedUsesUnderlyingFunctionCalcValue) +{ + // the FunctionBasedPath ultimately calculates its lengthening speed + // by using the underlying Function object, rather than computing it + // itself + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); + + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, m.coordinateAbsPaths}; + m.model.addComponent(fbp); + + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 0); + + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); + + double lengtheningSpeed = fbp->getLengtheningSpeed(s); + + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 1); + SimTK_TEST(lengtheningSpeed == fn.m_SharedData->derivative); + SimTK_TEST(fn.m_SharedData->lastCalcDerivativeIntArg == std::vector{0}); +} + +OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedIsCached) +{ + // FunctionBasedPath::getLengtheningSpeed should be "cached", depending on + // the simulation stage. + // + // this is an implementation detail that's good for implementors to know. Remove + // this test if it's a bad assumption. + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); + + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, m.coordinateAbsPaths}; + m.model.addComponent(fbp); + + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 0); + + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); + + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 0); + SimTK_TEST(fbp->getLengtheningSpeed(s) == fn.m_SharedData->value); + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 1); + fbp->getLengtheningSpeed(s); // should cache + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, AddInEquivalentForcesOnValidFBPDoesNotThrow) +{ + // calling `addInEquivalentForces` on a fully-initialized function-based + // path shouldn't throw, and should probably add *something* to the mobility + // forces outparam + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); // all coords affect this path + + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, m.coordinateAbsPaths}; + m.model.addComponent(fbp); + + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); + + double tension = 5.0; + SimTK::Vector_ bodyForces; + SimTK::Vector mobilityForces; + mobilityForces.resize(100); // required because impl doesn't actually add entries (no range checking) it's assigning a force per body index or something + + fbp->addInEquivalentForces(s, tension, bodyForces, mobilityForces); // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, AddInEquivalentForcesOnEmptyCoordlistWorks) +{ + // edge-case: you can still add in equivalent forces (i.e. nothing) when + // using an FBP that isn't parameterized by any Coordinates + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + MockPathFunction fn; + fn.m_SharedData->argumentSize = 0; // no coords affect this path + + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, {}}; + m.model.addComponent(fbp); + + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); + + double tension = 5.0; + SimTK::Vector_ bodyForces; + SimTK::Vector mobilityForces; + mobilityForces.resize(100); // required because impl doesn't actually add entries (no range checking) it's assigning a force per body index or something + + fbp->addInEquivalentForces(s, tension, bodyForces, mobilityForces); // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, ComputeMomentArmReturns0IfCalledWithNonAffectingCoord) +{ + // `computeMomentArm` may be called with a coordinate that does not actually + // affect the given FBP + // + // in that case, the returned moment arm shall always be 0.0 + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + MockPathFunction fn; + fn.m_SharedData->argumentSize = 4; // the first 4 coords affect the path + + auto affectingCoords = m.coordinateAbsPaths; + affectingCoords.resize(affectingCoords.size()-1); + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, affectingCoords}; + m.model.addComponent(fbp); + + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); + + const auto& nonAffectingCoord = m.model.getComponent(m.coordinateAbsPaths.back()); + + SimTK_TEST(fbp->computeMomentArm(s, nonAffectingCoord) == 0); +} + +OSIM_TEST(FunctionBasedPath, ComputeMomentArmCallsCalcDerivOfFunction) +{ + // this is an implementation detail - remove this test if the detail becomes non-true + // + // computing a moment arm should use the path's derivative to compute the + // moment arm. This test just tries to ensure that that's what's happening, + // as a sanity-check on the underlying plumbing + + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + MockPathFunction fn; + fn.m_SharedData->derivative = GenerateDouble(); + fn.m_SharedData->argumentSize = m.coordinateAbsPaths.size(); // all coords affect the path + + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, m.coordinateAbsPaths}; + m.model.addComponent(fbp); + + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); + + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 0); + int coordIdx = 0; + double momentArm = fbp->computeMomentArm(s, m.model.getComponent(m.coordinateAbsPaths[coordIdx])); + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 1); + SimTK_TEST(momentArm == fn.m_SharedData->derivative); +} + + + + +// HACK: test Joris's implementation here + +#include + + +// core runtime algorithm +namespace joris { + + static constexpr size_t g_MaxNumDimensions = 8; // important: this is an upper limit that's used for stack allocations + + // an `nPoints` evenly-spaced discretization of the range [begin, end] (inclusive) + // + // e.g. [0, 10], 0 points == [], step size = 10 + // , 1 point == [0], step size = 10 + // , 2 points == [0, 10], step size = 10 + // , 3 points == [0, 5, 10], step size = 5 + // , 4 points == [0, 3.33, 6.66, 10], step size = 3.33 + struct Discretization final { + double begin; + double end; + int nPoints; + }; + + double stepSize(const Discretization& d) + { + double diff = d.end - d.begin; + return d.nPoints <= 2 ? diff : diff/(d.nPoints-1); + } + + double realIndexOf(const Discretization& d, double v) + { + // solve: `x = d.begin + stepSize(d)*n` for `n` + return (v - d.begin) / stepSize(d); + } + + std::pair splitIntoWholeAndFractional(double v) + { + double wholePart; + double fractionalPart = std::modf(v, &wholePart); + return {static_cast(wholePart), fractionalPart}; + } + + std::pair splitIntoWholeAndFractionalForBetaCalc(const Discretization& d, double x) + { + std::pair wholeAndFrac = splitIntoWholeAndFractional(realIndexOf(d, x)); + + if (wholeAndFrac.first < 1) { + // edge-case: x is below the second graduation, use the 2nd discretization to ensure the fitting + // impl has access to all 4 (1 before, 2 after) datapoints + return {1, 0.0}; + } else if (wholeAndFrac.first > d.nPoints-3) { + // edge-case: x is greater than the third-to-last graduation, use the third-to-last discretization + // to ensurethe fitting impl. has asscess to all 4 (1 before, 2 after) datapoints + return {d.nPoints-3, 0.0}; + } else { + return wholeAndFrac; + } + } + + using Polynomial = std::array; + + Polynomial ComputeBeta(double frac) + { + // 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.0*frac + 1.0); + p[1] = -0.5 * fracMinusOne * (6.0*frac4 - 9.0*frac3 + 2.0*frac + 2.0); + p[2] = 0.5 * frac * (6.0*frac4 - 15.0*frac3 + 9.0*frac2 + frac + 1.0); + p[3] = -0.5 * fracMinusOne * frac3*(2.0*frac - 3.0); + + return p; + } + + Polynomial ComputeBetaDerivative(double frac) + { + // compute polynomial derivative based on fraction the point is toward the next point + + double frac2 = frac*frac; + double frac3 = frac2*frac; + double frac4 = frac3*frac; + + Polynomial p; + 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); + return p; + } + + // computes the interpolated Y value for a given permutation of X 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 + // + // `xVals` are the current values of each dimension (independent var: e.g. coordinate values - the "x"es) + double Impl_ComputeValueOrDeriv( + const std::vector& fittingDiscretizations, + const std::vector& fittingEvals, + const SimTK::Vector& xVals, + int derivIndex = -1) + { + SimTK_ASSERT_ALWAYS(!fittingDiscretizations.empty(), "this implementation requires that at least one X (e.g. coordinate) affects the path"); + SimTK_ASSERT_ALWAYS(xVals.size() == static_cast(fittingDiscretizations.size()), "You must call this function with the correct number of (precomputed) x values"); + SimTK_ASSERT_ALWAYS(xVals.size() < static_cast(g_MaxNumDimensions), "too many dimensions in this fit - the implementation cannot handle this many"); + + // 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 + std::array closestDiscretizationSteps; + std::array betas; + for (int dim = 0; dim < xVals.size(); ++dim) { + std::pair wholeFrac = splitIntoWholeAndFractionalForBetaCalc(fittingDiscretizations[dim], xVals[dim]); + closestDiscretizationSteps[dim] = wholeFrac.first; + betas[dim] = dim != derivIndex ? ComputeBeta(wholeFrac.second) : ComputeBetaDerivative(wholeFrac.second); + } + + // for each dim, permute through 4 (discretized) locations *around* the x 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 x as a sequence + // 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 dim = 0; dim < xVals.size(); ++dim) { + dimIdxOffsets[dim] = -1; + } + + // permute through all locations around the input value + // + // e.g. the location permutations for 3 dims 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; // result + int cnt = 0; // sanity-check counter + + // seems to be used in original implementation to scale the eval step size in the deriv calculation specifically + double evalScaler = derivIndex == -1 ? 1.0 : 1.0/stepSize(fittingDiscretizations[derivIndex]); + + while (dimIdxOffsets[0] < 3) { + + // compute `beta` (weighted coefficient per dimension) for this particular + // permutation's x 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 strideInFittingEvals = 1; + int idxInFittingEvals = 0; + + // go backwards, from least-significant dim (highest idx) to figure + // out where the evaluation is in the fittingEvals array + // + // this is so that we can compute the stride as the algorithm runs + for (int coord = xVals.size()-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]; // index into the polynomial + idxInFittingEvals += strideInFittingEvals * step; + strideInFittingEvals *= fittingDiscretizations[coord].nPoints; + } + + // equivalent to z += b*v, but handles rounding errors when the rhs + // is very small + z = std::fma(beta, evalScaler * fittingEvals.at(idxInFittingEvals), z); + + // increment the offsets + // + // this is effectively the step that permutes [-1, -1, 2] --> [-1, 0, -1] + { + int pos = xVals.size()-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*xVals.size()); + 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; + } +} + + +// TODO: test the low-level algorithm by providing it low-level data etc +namespace joris { + +} + + +// wireup of core runtime algorithm to OpenSim +namespace joris { + + // the underlying SimTK::Function that actually calls into the function + class JorisPathSimTKFunction : public SimTK::Function { + std::shared_ptr> _discretizations; + std::shared_ptr> _evaluations; + public: + JorisPathSimTKFunction( + std::shared_ptr> discretizations, + std::shared_ptr> evaluations) : + _discretizations{std::move(discretizations)}, + _evaluations{std::move(evaluations)} + { + } + + double calcValue(const SimTK::Vector& coordVals) const override + { + return Impl_ComputeValueOrDeriv(*_discretizations, *_evaluations, coordVals); + } + + double calcDerivative(const SimTK::Array_& derivComponents, const SimTK::Vector& coordVals) const override + { + SimTK_ASSERT_ALWAYS(derivComponents.size() == 1, "Can only find first-order derivative w.r.t. one coord"); + return Impl_ComputeValueOrDeriv(*_discretizations, *_evaluations, coordVals, derivComponents[0]); + } + + int getArgumentSize() const override + { + return static_cast(_discretizations->size()); + } + + int getMaxDerivativeOrder() const override + { + return 1; + } + }; + + using namespace OpenSim; // required by the property macros... + + class FunctionBasedPathDiscretization : public OpenSim::Component { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathDiscretization, OpenSim::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); + } + }; + + class FunctionBasedPathDiscretizationSet : public OpenSim::Set { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathDiscretizationSet, OpenSim::Set); + }; + + class JorisPathFunction final : public OpenSim::Function { + OpenSim_DECLARE_CONCRETE_OBJECT(JorisPathFunction, OpenSim::Function); + // TODO: this needs to have PROPERTYs to store the discretizations + evaluations + public: + std::shared_ptr> _discretizations; + std::shared_ptr> _evaluations; + public: + JorisPathFunction() : + _discretizations{std::make_shared>()}, + _evaluations{std::make_shared>()} + { + } + + JorisPathFunction( + std::shared_ptr> discretizations, + std::shared_ptr> evaluations) : + _discretizations{std::move(discretizations)}, + _evaluations{std::move(evaluations)} + { + } + + SimTK::Function* createSimTKFunction() const override + { + return new JorisPathSimTKFunction{_discretizations, _evaluations}; + } + + // TODO: flash vectors from properties with `extendFinalizeFromProperties` + }; + + /** TODO: handle computing a fresh FBP from a PBP, flashing from props, etc. + + // 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 + void Impl_InitFromFBPProperties(JorisFBP& impl) + { + FunctionBasedPathDiscretizationSet const& discSet = impl.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) { + 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 = impl.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)); + } + } + */ +} + + +// TODO: integration test that quickly ensures the wireup is using Joris's alg +namespace joris { +} + + +// TODO: code that compiles a new "FunctionBasedPath" from a "PointBasedPath" +namespace joris { + static constexpr int g_MaxCoordsThatCanAffectPathDefault = static_cast(g_MaxNumDimensions); + 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) + 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) + 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; + } + + // compute ideal discretization of the given coordinate + 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.nPoints = numDiscretizationSteps - 3; + double step = stepSize(d); + + // expand range slightly in either direction to ensure interpolation is + // clean around the edges + d.begin -= step; + d.end += 2.0 * step; + d.nPoints += 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)] + 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_MaxNumDimensions, 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].nPoints; + } + 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].nPoints) { + + // set all coordinate values for this step + for (size_t coord = 0; coord < ncoords; ++coord) { + Discretization const& discr = discs[coord]; + + double stepSz = stepSize(discr); + 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].nPoints) { + discStepIdx[pos] = 0; // overflow + ++discStepIdx[pos-1]; // carry + --pos; + } + } + + SimTK_ASSERT_ALWAYS(discStepIdx[0] == discs[0].nPoints, "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 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() : + maxCoordsThatCanAffectPath{g_MaxCoordsThatCanAffectPathDefault}, + numProbingDiscretizations{g_NumProbingDiscretizationsDefault}, + minProbingMomentArmChange{g_MinProbingMomentArmChangeDefault}, + numDiscretizationStepsPerDimension{g_NumDiscretizationStepsPerDimensionDefault} + { + } + }; + + /* todo + std::unique_ptr 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 JorisFBP{}}; + + JorisFBP& impl = *fbp; + + // compute underlying impl data from the PBP + if (!Impl_ComputeFromPBP(impl, model, pbp, params)) { + return nullptr; + } + + // write impl discretizations into the `Discretizations` property + 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->evals) { + evalsProp.appendValue(eval); + } + + return fbp; + } + + // 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 + bool Impl_ComputeFromPBP( + const OpenSim::Model& model, + const OpenSim::PointBasedPath& pbp, + const FittingParams& params, + std::vector& discretizationsOut, + std::vector& evalsOut, + std::vector& coordAbsPathsOut) + { + // 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; + } + */ +} + +// TODO: test fitting some basic `PointBasedPath`s using the funciton-fitting implementation +namespace joris { +} + +// TODO: implement conversion tool +namespace joris { +} + +// TODO: test conversion tool works as intended +namespace joris { +} + +// TODO: implement CLI tool +namespace joris { +} + +// TODO: implement CLI tests +namespace joris { +} + +int main() +{ + return RunAllTests(); +} diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 17b773ad80..ae96d25d75 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -53,6 +53,8 @@ #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" #include "Model/GeometryPath.h" +#include "Model/PointBasedPath.h" +#include "Model/FunctionBasedPath.h" #include "Model/PrescribedForce.h" #include "Model/PointToPointSpring.h" #include "Model/ExpressionBasedPointToPointForce.h"