From c72cc6c219d93c86d04a47d66c733ab94616696c Mon Sep 17 00:00:00 2001 From: Travis CI Date: Tue, 30 Nov 2021 11:40:57 +0100 Subject: [PATCH 01/12] Further little cleanups --- OpenSim/Simulation/Model/GeometryPath.cpp | 1121 ++++++++++----------- OpenSim/Simulation/Model/GeometryPath.h | 353 ++++--- 2 files changed, 730 insertions(+), 744 deletions(-) diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 2953703bb7..47e68208a7 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -31,215 +31,281 @@ #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() + +const PathPointSet& OpenSim::GeometryPath::getPathPointSet() const { - Super::extendFinalizeFromProperties(); + return get_PathPointSet(); +} - 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()); - } - } +PathPointSet& OpenSim::GeometryPath::updPathPointSet() +{ + return upd_PathPointSet(); } -void GeometryPath::extendConnectToModel(Model& aModel) +AbstractPathPoint* GeometryPath::addPathPoint(const SimTK::State& s, + int aIndex, + const PhysicalFrame& frame) { - Super::extendConnectToModel(aModel); + 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); - 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.") + // Rename the path points starting at this new one. + namePathPoints(aIndex); - // Name the path points based on the current path - // (i.e., the set of currently active points is numbered - // 1, 2, 3, ...). - namePathPoints(0); + // 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; i_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); + PathPoint* newPoint = new PathPoint(); + newPoint->setParentFrame(frame); + newPoint->setName(proposedName); + newPoint->setLocation(aPositionOnBody); + upd_PathPointSet().adoptAndAppend(newPoint); - // 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); + return newPoint; } - void GeometryPath::extendInitStateFromProperties(SimTK::State& s) const +bool GeometryPath::canDeletePathPoint(int aIndex) { - Super::extendInitStateFromProperties(s); - markCacheVariableValid(s, _colorCV); // it is OK at its default value + // 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; } -//------------------------------------------------------------------------------ -// 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; } +bool GeometryPath::deletePathPoint(const SimTK::State& s, int aIndex) +{ + if (!canDeletePathPoint(aIndex)) { + return false; + } - const Array& pathPoints = getCurrentPath(state); + upd_PathPointSet().remove(aIndex); - assert(pathPoints.size() > 1); + // rename the path points starting at the deleted position + namePathPoints(aIndex); - const AbstractPathPoint* lastPoint = pathPoints[0]; - MobilizedBodyIndex mbix(0); + // 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; igetLocationInGround(state); - if (hints.get_show_path_points()) - DefaultGeometry::drawPathPoint(mbix, lastPos, getColor(state), appendToThis); + if ( (startPoint != -1 && aIndex < startPoint) + || (startPoint > get_PathPointSet().getSize())) + get_PathWrapSet().get(i).setStartPoint(s, startPoint - 1); - Vec3 pos; + if ( endPoint > 1 + && aIndex <= endPoint + && ( (endPoint > startPoint) + || (endPoint > get_PathPointSet().getSize()))) + get_PathWrapSet().get(i).setEndPoint(s, endPoint - 1); + } - for (int i = 1; i < pathPoints.getSize(); ++i) { - AbstractPathPoint* point = pathPoints[i]; - PathWrapPoint* pwp = dynamic_cast(point); + return true; +} - 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; j(aOldPathPoint); + ConditionalPathPoint* newVia = + dynamic_cast(aNewPathPoint); + if (oldVia == NULL && newVia != NULL) { + for (int i=0; i + (&get_PathPointSet().get(i)) == NULL) + count++; + } } - } - else { // otherwise a regular PathPoint so just draw its location - pos = point->getLocationInGround(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; + } else { + count = 2; + } + if (count >= 2 && index >= 0) { + upd_PathPointSet().set(index, aNewPathPoint, true); + //computePath(s); + return true; } } + return false; } -//_____________________________________________________________________________ -/* - * Connect properties to local pointers. - */ -void GeometryPath::constructProperties() +const OpenSim::Array& GeometryPath::getCurrentPath( + const SimTK::State& s) const { - constructProperty_PathPointSet(PathPointSet()); + computePath(s); // compute checks if path needs to be recomputed + return getCacheVariableValue< Array >(s, _currentPathCV); +} - constructProperty_PathWrapSet(PathWrapSet()); - - Appearance appearance; - appearance.set_color(SimTK::Gray); - constructProperty_Appearance(appearance); + +const PathWrapSet& OpenSim::GeometryPath::getWrapSet() const +{ + return get_PathWrapSet(); } -//_____________________________________________________________________________ -/* - * 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) +PathWrapSet& OpenSim::GeometryPath::updWrapSet() { - 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); - } + return upd_PathWrapSet(); +} + +void GeometryPath::addPathWrap(WrapObject& aWrapObject) +{ + PathWrap* newWrap = new PathWrap(); + newWrap->setWrapObject(aWrapObject); + newWrap->setMethod(PathWrap::hybrid); + upd_PathWrapSet().adoptAndAppend(newWrap); + finalizeFromProperties(); +} + +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); } } -//_____________________________________________________________________________ -/* - * 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 +void GeometryPath::moveDownPathWrap(const SimTK::State& s, int aIndex) { - computePath(s); // compute checks if path needs to be recomputed - return getCacheVariableValue< Array >(s, "current_path"); + 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); + } } -// 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 +void GeometryPath::deletePathWrap(const SimTK::State& s, int aIndex) +{ + upd_PathWrapSet().remove(aIndex); +} + + +const SimTK::Vec3& OpenSim::GeometryPath::getDefaultColor() const +{ + return get_Appearance().get_color(); +} + +void OpenSim::GeometryPath::setDefaultColor(const SimTK::Vec3& color) +{ + updProperty_Appearance().setValueIsDefault(false); + upd_Appearance().set_color(color); +} + + +Vec3 GeometryPath::getColor(const SimTK::State& s) const +{ + return getCacheVariableValue(s, _colorCV); +} + +void GeometryPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const +{ + setCacheVariableValue(s, _colorCV, color); +} + + +double GeometryPath::getLength(const SimTK::State& s) const +{ + computePath(s); // compute checks if path needs to be recomputed + return getCacheVariableValue(s, _lengthCV); +} + +void GeometryPath::setLength(const SimTK::State& s, double length) const +{ + setCacheVariableValue(s, _lengthCV, length); +} + + +double GeometryPath::getLengtheningSpeed(const SimTK::State& s) const +{ + computeLengtheningSpeed(s); + return getCacheVariableValue(s, _speedCV); +} + +void GeometryPath::setLengtheningSpeed(const SimTK::State& s, double speed) const +{ + setCacheVariableValue(s, _speedCV, speed); +} + + +double GeometryPath::getPreScaleLength(const SimTK::State& s) const +{ + return _preScaleLength; +} + +void GeometryPath::setPreScaleLength(const SimTK::State& s, double length) +{ + _preScaleLength = length; +} + +void GeometryPath::getPointForceDirections( + const SimTK::State& s, + OpenSim::Array *rPFDs) const { int i; AbstractPathPoint* start; @@ -250,10 +316,10 @@ getPointForceDirections(const SimTK::State& s, int np = currentPath.getSize(); rPFDs->ensureCapacity(np); - + for (i = 0; i < np; i++) { - PointForceDirection *pfd = - new PointForceDirection(currentPath[i]->getLocation(s), + PointForceDirection *pfd = + new PointForceDirection(currentPath[i]->getLocation(s), currentPath[i]->getParentFrame(), Vec3(0)); rPFDs->append(pfd); } @@ -272,7 +338,7 @@ getPointForceDirections(const SimTK::State& s, // 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); @@ -291,17 +357,16 @@ getPointForceDirections(const SimTK::State& s, direction = direction.normalize(); } - // Get resultant direction at each point + // Get resultant direction at each point rPFDs->get(i)->addToDirection(direction); rPFDs->get(i+1)->addToDirection(-direction); } } } -/* add in the equivalent spatial forces on bodies for an applied tension - along the GeometryPath to a set of bodyForces */ -void GeometryPath::addInEquivalentForces(const SimTK::State& s, - const double& tension, +void GeometryPath::addInEquivalentForces( + const SimTK::State& s, + double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const { @@ -312,12 +377,12 @@ void GeometryPath::addInEquivalentForces(const SimTK::State& s, const Array& currentPath = getCurrentPath(s); int np = currentPath.getSize(); - const SimTK::SimbodyMatterSubsystem& matter = + 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 + // 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 @@ -389,18 +454,18 @@ void GeometryPath::addInEquivalentForces(const SimTK::State& s, // 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 + // 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; + 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(), + mpbod.applyOneMobilityForce(s, + mppo->getXCoordinate().getMobilizerQIndex(), fo, mobilityForces); } @@ -412,405 +477,186 @@ void GeometryPath::addInEquivalentForces(const SimTK::State& s, const SimTK::MobilizedBody& mpbod = matter.getMobilizedBody(mppf->getXCoordinate().getBodyIndex()); - mpbod.applyOneMobilityForce(s, - mppf->getXCoordinate().getMobilizerQIndex(), + mpbod.applyOneMobilityForce(s, + mppf->getXCoordinate().getMobilizerQIndex(), ff, mobilityForces); } - } - } -} - -//_____________________________________________________________________________ -/* - * 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 -{ - // Check if the current path needs to recomputed. - computePath(s); -} - -//============================================================================= -// GET -//============================================================================= -//----------------------------------------------------------------------------- -// LENGTH -//----------------------------------------------------------------------------- -//_____________________________________________________________________________ -/* - * Compute the total length of the path. - * - * @return Total length of the path. - */ -double GeometryPath::getLength( const SimTK::State& s) const -{ - computePath(s); // compute checks if path needs to be recomputed - return getCacheVariableValue(s, _lengthCV); -} - -void GeometryPath::setLength( const SimTK::State& s, double length ) const -{ - setCacheVariableValue(s, _lengthCV, length); -} - -void GeometryPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const -{ - setCacheVariableValue(s, _colorCV, color); -} - -Vec3 GeometryPath::getColor(const SimTK::State& s) const -{ - return getCacheVariableValue(s, _colorCV); -} - -//_____________________________________________________________________________ -/* - * Compute the lengthening speed of the path. - * - * @return lengthening speed of the path. - */ -double GeometryPath::getLengtheningSpeed( const SimTK::State& s) const -{ - computeLengtheningSpeed(s); - return getCacheVariableValue(s, _speedCV); -} -void GeometryPath::setLengtheningSpeed( const SimTK::State& s, double speed ) const -{ - setCacheVariableValue(s, _speedCV, speed); -} - -void GeometryPath::setPreScaleLength( const SimTK::State& s, double length ) { - _preScaleLength = length; -} -double GeometryPath::getPreScaleLength( const SimTK::State& s) const { - return _preScaleLength; -} - -//============================================================================= -// UTILITY -//============================================================================= -//_____________________________________________________________________________ -/* - * Add a new path point, with default location, to the path. - * - * @param aIndex The position in the pathPointSet to put the new point in. - * @param frame The frame to attach the point to. - * @return Pointer to the newly created path point. - */ -AbstractPathPoint* GeometryPath:: -addPathPoint(const SimTK::State& s, int aIndex, const PhysicalFrame& frame) -{ - PathPoint* newPoint = new PathPoint(); - newPoint->setParentFrame(frame); - Vec3 newLocation(0.0); - // Note: placeNewPathPoint() returns a location by reference. - // It computes a new location according to the index where the new path point - // will be inserted along the path(among the other path points). - placeNewPathPoint(s, newLocation, aIndex, frame); - // Now set computed new location into the newPoint - newPoint->setLocation(newLocation); - upd_PathPointSet().insert(aIndex, newPoint); - - // Rename the path points starting at this new one. - namePathPoints(aIndex); - - // Update start point and end point in the wrap instances so that they - // refer to the same path points they did before the new point - // was added. These indices are 1-based. - aIndex++; - for (int i=0; isetParentFrame(frame); - newPoint->setName(proposedName); - newPoint->setLocation(aPositionOnBody); - upd_PathPointSet().adoptAndAppend(newPoint); - - return newPoint; -} - -//_____________________________________________________________________________ -/* - * Determine an appropriate default XYZ location for a new path point. - * Note that this method is internal and should not be called directly on a new - * point as the point is not really added to the path (done in addPathPoint() - * instead) - * @param aOffset The XYZ location to be determined. - * @param aIndex The position in the pathPointSet to put the new point in. - * @param frame The body to attach the point to. - */ -void GeometryPath:: -placeNewPathPoint(const SimTK::State& s, SimTK::Vec3& aOffset, int aIndex, - const PhysicalFrame& frame) -{ - // The location of the point is determined by moving a 'distance' from 'base' - // along a vector from 'start' to 'end.' 'base' is the existing path point - // that is in or closest to the index aIndex. 'start' and 'end' are existing - // path points--which ones depends on where the new point is being added. - // 'distance' is 0.5 for points added to the middle of a path (so the point - // appears halfway between the two adjacent points), and 0.2 for points that - // are added to either end of the path. If there is only one point in the - // path, the new point is put 0.01 units away in all three dimensions. - if (get_PathPointSet().getSize() > 1) { - int start, end, base; - double distance; - if (aIndex == 0) { - start = 1; - end = 0; - base = end; - distance = 0.2; - } else if (aIndex >= get_PathPointSet().getSize()) { - start = aIndex - 2; - end = aIndex - 1; - base = end; - distance = 0.2; - } else { - start = aIndex; - end = aIndex - 1; - base = start; - distance = 0.5; } - - const Vec3 startPt = get_PathPointSet()[start].getLocation(s); - const Vec3 endPt = get_PathPointSet()[end].getLocation(s); - const Vec3 basePt = get_PathPointSet()[base].getLocation(s); - - Vec3 startPt2 = get_PathPointSet()[start].getParentFrame() - .findStationLocationInAnotherFrame(s, startPt, frame); - - Vec3 endPt2 = get_PathPointSet()[end].getParentFrame() - .findStationLocationInAnotherFrame(s, endPt, frame); - - aOffset = basePt + distance * (endPt2 - startPt2); - } else if (get_PathPointSet().getSize() == 1) { - aOffset= get_PathPointSet()[0].getLocation(s) + 0.01; - } - else { // first point, do nothing? } } -//_____________________________________________________________________________ -/* - * See if a path point can be deleted. All paths must have at least two - * active path points to define the path. - * - * @param aIndex The index of the point to delete. - * @return Whether or not the point can be deleted. - */ -bool GeometryPath::canDeletePathPoint( int aIndex) +double GeometryPath::computeMomentArm( + const SimTK::State& s, + const Coordinate& aCoord) const { - // A path point can be deleted only if there would remain - // at least two other fixed points. - int numOtherFixedPoints = 0; - for (int i = 0; i < get_PathPointSet().getSize(); i++) { - if (i != aIndex) { - if (!( get_PathPointSet().get(i).getConcreteClassName() - ==("ConditionalPathPoint"))) - numOtherFixedPoints++; - } - } - - if (numOtherFixedPoints >= 2) - return true; - - return false; -} - -//_____________________________________________________________________________ -/* - * Delete a path point. - * - * @param aIndex The index of the point to delete. - * @return Whether or not the point was deleted. - */ -bool GeometryPath::deletePathPoint(const SimTK::State& s, int aIndex) -{ - if (canDeletePathPoint(aIndex) == false) - return false; - - upd_PathPointSet().remove(aIndex); - - // rename the path points starting at the deleted position - namePathPoints(aIndex); - - // Update start point and end point in the wrap instances so that they - // refer to the same path points they did before the point was - // deleted. These indices are 1-based. If the point deleted is start - // point or end point, the path wrap range is made smaller by one point. - aIndex++; - for (int i=0; i get_PathPointSet().getSize())) - get_PathWrapSet().get(i).setStartPoint(s, startPoint - 1); - - if ( endPoint > 1 - && aIndex <= endPoint - && ( (endPoint > startPoint) - || (endPoint > get_PathPointSet().getSize()))) - get_PathWrapSet().get(i).setEndPoint(s, endPoint - 1); + if (!_maSolver) { + const_cast(this)->_maSolver.reset(new MomentArmSolver(*_model)); } - return true; + return _maSolver->solve(s, aCoord, *this); } -//_____________________________________________________________________________ -/* - * 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) + +void GeometryPath::extendPreScale(const SimTK::State& s, + const ScaleSet& scaleSet) { - 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; + Super::extendPreScale(s, scaleSet); + setPreScaleLength(s, getLength(s)); } -//_____________________________________________________________________________ -/* - * 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) +void GeometryPath::extendPostScale(const SimTK::State& s, + const ScaleSet& scaleSet) { - PathWrap* newWrap = new PathWrap(); - newWrap->setWrapObject(aWrapObject); - newWrap->setMethod(PathWrap::hybrid); - upd_PathWrapSet().adoptAndAppend(newWrap); - finalizeFromProperties(); + Super::extendPostScale(s, scaleSet); + computePath(s); } -//_____________________________________________________________________________ -/* - * 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) + +void GeometryPath::updateGeometry(const SimTK::State& s) const { - if (aIndex > 0) { - // Make sure wrap object is not deleted by remove(). - upd_PathWrapSet().setMemoryOwner(false); + // Check if the current path needs to recomputed. + computePath(s); +} - PathWrap& wrap = get_PathWrapSet().get(aIndex); - upd_PathWrapSet().remove(aIndex); - upd_PathWrapSet().insert(aIndex - 1, &wrap); - upd_PathWrapSet().setMemoryOwner(true); + +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()); + } } } -//_____________________________________________________________________________ -/* - * 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) +void GeometryPath::extendConnectToModel(Model& aModel) { - if (aIndex < get_PathWrapSet().getSize() - 1) { - // Make sure wrap object is not deleted by remove(). - upd_PathWrapSet().setMemoryOwner(false); + Super::extendConnectToModel(aModel); - PathWrap& wrap = get_PathWrapSet().get(aIndex); - upd_PathWrapSet().remove(aIndex); - upd_PathWrapSet().insert(aIndex + 1, &wrap); - upd_PathWrapSet().setMemoryOwner(true); - } + 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); } -//_____________________________________________________________________________ -/* - * Delete a wrap instance. - * - * @param aIndex The index of the wrap instance to delete. - */ -void GeometryPath::deletePathWrap(const SimTK::State& s, int aIndex) +void GeometryPath::extendAddToSystem(SimTK::MultibodySystem& system) const { - upd_PathWrapSet().remove(aIndex); + 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); } -//============================================================================== -// SCALING -//============================================================================== -void GeometryPath:: -extendPreScale(const SimTK::State& s, const ScaleSet& scaleSet) +void GeometryPath::extendInitStateFromProperties(SimTK::State& s) const { - Super::extendPreScale(s, scaleSet); - setPreScaleLength(s, getLength(s)); + 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 GeometryPath::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]; + 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; + } + } } -void GeometryPath:: -extendPostScale(const SimTK::State& s, const ScaleSet& scaleSet) + +// private methods + + +void GeometryPath::constructProperties() { - Super::extendPostScale(s, scaleSet); - computePath(s); + constructProperty_PathPointSet(PathPointSet()); + constructProperty_PathWrapSet(PathWrapSet()); + + Appearance appearance; + appearance.set_color(SimTK::Gray); + constructProperty_Appearance(appearance); } -//-------------------------------------------------------------------------- -// COMPUTATIONS -//-------------------------------------------------------------------------- -//============================================================================= -// PATH, WRAPPING, AND MOMENT ARM -//============================================================================= -//_____________________________________________________________________________ -/* - * Calculate the current path. - */ void GeometryPath::computePath(const SimTK::State& s) const { if (isCacheVariableValid(s, _currentPathCV)) { @@ -826,8 +672,8 @@ void GeometryPath::computePath(const SimTK::State& s) const 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, + + // 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); @@ -835,10 +681,6 @@ void GeometryPath::computePath(const SimTK::State& s) const markCacheVariableValid(s, _currentPathCV); } -//_____________________________________________________________________________ -/* - * Compute lengthening speed of the path. - */ void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const { if (isCacheVariableValid(s, _speedCV)) { @@ -848,7 +690,6 @@ void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const 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]); } @@ -856,15 +697,12 @@ void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const setLengtheningSpeed(s, speed); } -//_____________________________________________________________________________ -/* - * Apply the wrap objects to the current path. - */ -void GeometryPath:: -applyWrapObjects(const SimTK::State& s, Array& path) const +void GeometryPath::applyWrapObjects(const SimTK::State& s, + Array& path) const { - if (get_PathWrapSet().getSize() < 1) + if (get_PathWrapSet().getSize() < 1) { return; + } WrapResult best_wrap; Array result, order; @@ -902,27 +740,27 @@ applyWrapObjects(const SimTK::State& s, Array& path) const } if (wo->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[] + // 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 + // 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 + // 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 + ? 0 : ws.getStartPoint() - 1); const int wrapEnd = (ws.getEndPoint() < 1 - ? get_PathPointSet().getSize() - 1 + ? get_PathPointSet().getSize() - 1 : ws.getEndPoint() - 1); - // 2. Scan forward from wrapStart in get_PathPointSet() to find + // 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++) @@ -932,7 +770,7 @@ applyWrapObjects(const SimTK::State& s, Array& path) const return; const AbstractPathPoint* const smp = &get_PathPointSet().get(jfwd); - // 3. Scan backwards from wrapEnd in get_PathPointSet() to find + // 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--) @@ -953,11 +791,11 @@ applyWrapObjects(const SimTK::State& s, Array& path) const 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 + // 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++) { @@ -965,47 +803,47 @@ applyWrapObjects(const SimTK::State& s, Array& path) const // 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() + 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), + 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 + // "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 + // 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 + // taken as the mandatory wrap (this is considered // an ill-conditioned case). best_wrap = wr; - // Store the best wrap in the pathWrap for possible + // 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 + // 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 = + 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 + // 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 + // The wrap was not shorter than the current + // minimum, so just free the wrap points that // were allocated. wr.wrap_pts.setSize(0); } @@ -1028,13 +866,13 @@ applyWrapObjects(const SimTK::State& s, Array& path) const 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 + // 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, + // convert(ms->modelnum, wrapPath.get(j), wo->segment, // ms->ground_segment); // } @@ -1051,7 +889,7 @@ applyWrapObjects(const SimTK::State& s, Array& path) const } } - const double length = calcLengthAfterPathComputation(s, path); + const double length = calcLengthAfterPathComputation(s, path); if (std::abs(length - last_length) < 0.0005) { break; } else { @@ -1062,7 +900,7 @@ applyWrapObjects(const SimTK::State& s, Array& path) const // 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 + if ( result[0] == WrapObject::noWrap && result[1] == WrapObject::insideRadius) { order[0] = 1; @@ -1082,15 +920,16 @@ applyWrapObjects(const SimTK::State& s, Array& path) const } } -//_____________________________________________________________________________ /* * _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 +double GeometryPath::calcPathLengthChange( + const SimTK::State& s, + const WrapObject& wo, + const WrapResult& wr, + const Array& path) const { const AbstractPathPoint* pt1 = path.get(wr.startPoint); const AbstractPathPoint* pt2 = path.get(wr.endPoint); @@ -1104,14 +943,13 @@ calcPathLengthChange(const SimTK::State& s, const WrapObject& wo, return wrap_length - straight_length; // return absolute diff, not relative } -//_____________________________________________________________________________ /* * Compute the total length of the path. This function * assumes that the path has already been updated. */ -double GeometryPath:: -calcLengthAfterPathComputation(const SimTK::State& s, - const Array& currentPath) const +double GeometryPath::calcLengthAfterPathComputation( + const SimTK::State& s, + const Array& currentPath) const { double length = 0.0; @@ -1120,11 +958,11 @@ calcLengthAfterPathComputation(const SimTK::State& s, 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 + // 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()) + if ( p1->getWrapObject() + && p2->getWrapObject() + && p1->getWrapObject() == p2->getWrapObject()) { const PathWrapPoint* smwp = dynamic_cast(p2); if (smwp) @@ -1138,24 +976,86 @@ calcLengthAfterPathComputation(const SimTK::State& s, return( length ); } -//_____________________________________________________________________________ /* - * Compute the path's moment arms for specified coordinate. + * 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 aCoord, the coordinate - */ -double GeometryPath:: -computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const + * @param aStartingIndex The index of the first path point to name. + */ +void GeometryPath::namePathPoints(int aStartingIndex) { - if (!_maSolver) - const_cast(this)->_maSolver.reset(new MomentArmSolver(*_model)); + 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); + } + } +} - return _maSolver->solve(s, aCoord, *this); +/* + * Determine an appropriate default XYZ location for a new path point. + * Note that this method is internal and should not be called directly on a new + * point as the point is not really added to the path (done in addPathPoint() + * instead) + * @param aOffset The XYZ location to be determined. + * @param aIndex The position in the pathPointSet to put the new point in. + * @param frame The body to attach the point to. + */ +void GeometryPath::placeNewPathPoint( + const SimTK::State& s, + SimTK::Vec3& aOffset, + int aIndex, + const PhysicalFrame& frame) +{ + // The location of the point is determined by moving a 'distance' from 'base' + // along a vector from 'start' to 'end.' 'base' is the existing path point + // that is in or closest to the index aIndex. 'start' and 'end' are existing + // path points--which ones depends on where the new point is being added. + // 'distance' is 0.5 for points added to the middle of a path (so the point + // appears halfway between the two adjacent points), and 0.2 for points that + // are added to either end of the path. If there is only one point in the + // path, the new point is put 0.01 units away in all three dimensions. + if (get_PathPointSet().getSize() > 1) { + int start, end, base; + double distance; + if (aIndex == 0) { + start = 1; + end = 0; + base = end; + distance = 0.2; + } else if (aIndex >= get_PathPointSet().getSize()) { + start = aIndex - 2; + end = aIndex - 1; + base = end; + distance = 0.2; + } else { + start = aIndex; + end = aIndex - 1; + base = start; + distance = 0.5; + } + + const Vec3 startPt = get_PathPointSet()[start].getLocation(s); + const Vec3 endPt = get_PathPointSet()[end].getLocation(s); + const Vec3 basePt = get_PathPointSet()[base].getLocation(s); + + Vec3 startPt2 = get_PathPointSet()[start].getParentFrame() + .findStationLocationInAnotherFrame(s, startPt, frame); + + Vec3 endPt2 = get_PathPointSet()[end].getParentFrame() + .findStationLocationInAnotherFrame(s, endPt, frame); + + aOffset = basePt + distance * (endPt2 - startPt2); + } else if (get_PathPointSet().getSize() == 1) { + aOffset= get_PathPointSet()[0].getLocation(s) + 0.01; + } + else { // first point, do nothing? + } } -//_____________________________________________________________________________ -// 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) { if (versionNumber < XMLDocument::getLatestVersion()) { @@ -1188,6 +1088,7 @@ void GeometryPath::updateFromXMLNode(SimTK::Xml::Element& aNode, int versionNumb } } } + // Call base class now assuming aNode has been corrected for current version Super::updateFromXMLNode(aNode, versionNumber); } diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index 045efe5888..6e6cc1c8c1 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 @@ -47,23 +45,29 @@ 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. + */ 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); + +//============================================================================= +// OUTPUTS +//============================================================================= + OpenSim_DECLARE_OUTPUT(length, + double, + getLength, + SimTK::Stage::Position); + + OpenSim_DECLARE_OUTPUT(lengthening_speed, + double, + getLengtheningSpeed, + SimTK::Stage::Velocity); //============================================================================= // DATA @@ -95,161 +99,242 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); //============================================================================= // METHODS //============================================================================= - //-------------------------------------------------------------------------- - // CONSTRUCTION - //-------------------------------------------------------------------------- public: GeometryPath(); - ~GeometryPath() override = default; - const PathPointSet& getPathPointSet() const { return get_PathPointSet(); } - PathPointSet& updPathPointSet() { return upd_PathPointSet(); } - const PathWrapSet& getWrapSet() const { return get_PathWrapSet(); } - PathWrapSet& updWrapSet() { return upd_PathWrapSet(); } - void addPathWrap(WrapObject& aWrapObject); - - //-------------------------------------------------------------------------- - // UTILITY - //-------------------------------------------------------------------------- - AbstractPathPoint* addPathPoint(const SimTK::State& s, int index, - const PhysicalFrame& frame); - AbstractPathPoint* appendNewPathPoint(const std::string& proposedName, - const PhysicalFrame& frame, const SimTK::Vec3& locationOnFrame); - bool canDeletePathPoint( int index); + const PathPointSet& getPathPointSet() const; + 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. + */ + AbstractPathPoint* addPathPoint( + const SimTK::State& s, + int index, + const PhysicalFrame& frame); + 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. + */ + 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. + */ 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. + */ + 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. + */ + const Array& getCurrentPath(const SimTK::State& s) const; + + + const PathWrapSet& getWrapSet() const; + 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. + */ + 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. + */ 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. + */ void moveDownPathWrap(const SimTK::State& s, int index); + /** + * Delete a wrap instance. + * + * @param aIndex The index of the wrap instance to delete. + */ 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() **/ + /** + * Get the default color of the path. + * + * 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, which may have changed. + */ + const SimTK::Vec3& getDefaultColor() const; + + /** + * Set the default color of the path. + * + * 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); + + /** + * Get the current color of the path. + * + * Internally, gets the 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; + /** + * Set the current color of the path. + * + * Internally, sets 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. Therefore, 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; + + double getLength(const SimTK::State& s) const; + void setLength(const SimTK::State& s, double length) const; + /** + * Compute the lengthening speed of the path. + * + * @return lengthening speed of the path. + */ 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 setLengtheningSpeed(const SimTK::State& s, double speed) const; + + double getPreScaleLength(const SimTK::State& s) const; + void setPreScaleLength(const SimTK::State& s, double preScaleLength); + + /** + * 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! + */ + 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, + double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const; - - //-------------------------------------------------------------------------- - // COMPUTATIONS - //-------------------------------------------------------------------------- - virtual double computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const; + /** + * Compute the path's moment arms for specified coordinate. + * + * @param aCoord, the coordinate + */ + 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. */ + /** + * 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. */ + /** + * 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) + /** + * Updates the geometry attached to the path (location of path points and + * connecting segments all in global/inertial frame) + */ virtual void updateGeometry(const SimTK::State& s) const; protected: - // ModelComponent interface. + void extendFinalizeFromProperties() override; void extendConnectToModel(Model& aModel) override; - void extendInitStateFromProperties(SimTK::State& s) const override; void extendAddToSystem(SimTK::MultibodySystem& system) const override; + void extendInitStateFromProperties(SimTK::State& s) 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; + 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 constructProperties(); + + 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; + 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; -//============================================================================= -}; // END of class GeometryPath -//============================================================================= -//============================================================================= -} // end of namespace OpenSim + 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 // OPENSIM_GEOMETRY_PATH_H_ From 310abe1d5453b5cdcffda046af7e389da3a38a56 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Tue, 30 Nov 2021 15:45:52 +0100 Subject: [PATCH 02/12] Virtualized GeometryPath and implemented PointBasedPath --- Bindings/OpenSimHeaders_simulation.h | 1 + OpenSim/Simulation/Model/Appearance.h | 1 + .../Model/Blankevoort1991Ligament.cpp | 6 +- OpenSim/Simulation/Model/GeometryPath.cpp | 1063 +--------------- OpenSim/Simulation/Model/GeometryPath.h | 160 +-- OpenSim/Simulation/Model/Ligament.cpp | 14 +- OpenSim/Simulation/Model/PathActuator.cpp | 4 +- OpenSim/Simulation/Model/PathSpring.cpp | 5 +- OpenSim/Simulation/Model/PointBasedPath.cpp | 1092 +++++++++++++++++ OpenSim/Simulation/Model/PointBasedPath.h | 177 +++ .../RegisterTypes_osimSimulation.cpp | 6 +- OpenSim/Simulation/osimSimulation.h | 1 + 12 files changed, 1340 insertions(+), 1190 deletions(-) create mode 100644 OpenSim/Simulation/Model/PointBasedPath.cpp create mode 100644 OpenSim/Simulation/Model/PointBasedPath.h diff --git a/Bindings/OpenSimHeaders_simulation.h b/Bindings/OpenSimHeaders_simulation.h index f22f948d4c..d3c5580ae5 100644 --- a/Bindings/OpenSimHeaders_simulation.h +++ b/Bindings/OpenSimHeaders_simulation.h @@ -110,6 +110,7 @@ #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/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 47e68208a7..497f898971 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -21,1074 +21,15 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================= -// INCLUDES -//============================================================================= #include "GeometryPath.h" -#include "ConditionalPathPoint.h" -#include "MovingPathPoint.h" -#include "PointForceDirection.h" -#include -#include "Model.h" -using namespace OpenSim; -using namespace SimTK; -using SimTK::Vec3; +#include -GeometryPath::GeometryPath() : - ModelComponent(), - _preScaleLength(0.0) +OpenSim::GeometryPath::GeometryPath() : ModelComponent{} { setAuthors("Peter Loan"); - constructProperties(); -} - - -const PathPointSet& OpenSim::GeometryPath::getPathPointSet() const -{ - return get_PathPointSet(); -} - -PathPointSet& OpenSim::GeometryPath::updPathPointSet() -{ - return upd_PathPointSet(); -} - -AbstractPathPoint* GeometryPath::addPathPoint(const SimTK::State& s, - int aIndex, - const PhysicalFrame& frame) -{ - PathPoint* newPoint = new PathPoint(); - newPoint->setParentFrame(frame); - Vec3 newLocation(0.0); - // Note: placeNewPathPoint() returns a location by reference. - // It computes a new location according to the index where the new path point - // will be inserted along the path(among the other path points). - placeNewPathPoint(s, newLocation, aIndex, frame); - // Now set computed new location into the newPoint - newPoint->setLocation(newLocation); - upd_PathPointSet().insert(aIndex, newPoint); - - // Rename the path points starting at this new one. - namePathPoints(aIndex); - - // Update start point and end point in the wrap instances so that they - // refer to the same path points they did before the new point - // was added. These indices are 1-based. aIndex++; - for (int i=0; isetParentFrame(frame); - newPoint->setName(proposedName); - newPoint->setLocation(aPositionOnBody); - upd_PathPointSet().adoptAndAppend(newPoint); - - return newPoint; -} - -bool GeometryPath::canDeletePathPoint(int aIndex) -{ - // A path point can be deleted only if there would remain - // at least two other fixed points. - int numOtherFixedPoints = 0; - for (int i = 0; i < get_PathPointSet().getSize(); i++) { - if (i != aIndex) { - if (!( get_PathPointSet().get(i).getConcreteClassName() - ==("ConditionalPathPoint"))) - numOtherFixedPoints++; - } - } - - if (numOtherFixedPoints >= 2) - return true; - - return false; -} - -bool GeometryPath::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 GeometryPath::replacePathPoint(const SimTK::State& s, - AbstractPathPoint* aOldPathPoint, - AbstractPathPoint* aNewPathPoint) -{ - if (aOldPathPoint != NULL && aNewPathPoint != NULL) { - int count = 0; - int index = get_PathPointSet().getIndex(aOldPathPoint); - // If you're switching from non-via to via, check to make sure that the - // path will be left with at least 2 non-via points. - ConditionalPathPoint* oldVia = - dynamic_cast(aOldPathPoint); - ConditionalPathPoint* newVia = - dynamic_cast(aNewPathPoint); - if (oldVia == NULL && newVia != NULL) { - for (int i=0; i - (&get_PathPointSet().get(i)) == NULL) - count++; - } - } - } else { - count = 2; - } - if (count >= 2 && index >= 0) { - upd_PathPointSet().set(index, aNewPathPoint, true); - //computePath(s); - return true; - } - } - return false; -} - -const OpenSim::Array& GeometryPath::getCurrentPath( - const SimTK::State& s) const -{ - computePath(s); // compute checks if path needs to be recomputed - return getCacheVariableValue< Array >(s, _currentPathCV); -} - - -const PathWrapSet& OpenSim::GeometryPath::getWrapSet() const -{ - return get_PathWrapSet(); -} - -PathWrapSet& OpenSim::GeometryPath::updWrapSet() -{ - return upd_PathWrapSet(); -} - -void GeometryPath::addPathWrap(WrapObject& aWrapObject) -{ - PathWrap* newWrap = new PathWrap(); - newWrap->setWrapObject(aWrapObject); - newWrap->setMethod(PathWrap::hybrid); - upd_PathWrapSet().adoptAndAppend(newWrap); - finalizeFromProperties(); -} - -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); - } -} - -void GeometryPath::moveDownPathWrap(const SimTK::State& s, int aIndex) -{ - if (aIndex < get_PathWrapSet().getSize() - 1) { - // Make sure wrap object is not deleted by remove(). - upd_PathWrapSet().setMemoryOwner(false); - - PathWrap& wrap = get_PathWrapSet().get(aIndex); - upd_PathWrapSet().remove(aIndex); - upd_PathWrapSet().insert(aIndex + 1, &wrap); - upd_PathWrapSet().setMemoryOwner(true); - } -} - -void GeometryPath::deletePathWrap(const SimTK::State& s, int aIndex) -{ - upd_PathWrapSet().remove(aIndex); -} - - -const SimTK::Vec3& OpenSim::GeometryPath::getDefaultColor() const -{ - return get_Appearance().get_color(); -} - -void OpenSim::GeometryPath::setDefaultColor(const SimTK::Vec3& color) -{ - updProperty_Appearance().setValueIsDefault(false); - upd_Appearance().set_color(color); -} - - -Vec3 GeometryPath::getColor(const SimTK::State& s) const -{ - return getCacheVariableValue(s, _colorCV); -} - -void GeometryPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const -{ - setCacheVariableValue(s, _colorCV, color); -} - - -double GeometryPath::getLength(const SimTK::State& s) const -{ - computePath(s); // compute checks if path needs to be recomputed - return getCacheVariableValue(s, _lengthCV); -} - -void GeometryPath::setLength(const SimTK::State& s, double length) const -{ - setCacheVariableValue(s, _lengthCV, length); -} - - -double GeometryPath::getLengtheningSpeed(const SimTK::State& s) const -{ - computeLengtheningSpeed(s); - return getCacheVariableValue(s, _speedCV); -} - -void GeometryPath::setLengtheningSpeed(const SimTK::State& s, double speed) const -{ - setCacheVariableValue(s, _speedCV, speed); -} - - -double GeometryPath::getPreScaleLength(const SimTK::State& s) const -{ - return _preScaleLength; -} - -void GeometryPath::setPreScaleLength(const SimTK::State& s, double length) -{ - _preScaleLength = length; -} - -void GeometryPath::getPointForceDirections( - const SimTK::State& s, - OpenSim::Array *rPFDs) const -{ - int i; - AbstractPathPoint* start; - AbstractPathPoint* end; - const OpenSim::PhysicalFrame* startBody; - const OpenSim::PhysicalFrame* endBody; - const Array& currentPath = getCurrentPath(s); - - int np = currentPath.getSize(); - rPFDs->ensureCapacity(np); - - for (i = 0; i < np; i++) { - PointForceDirection *pfd = - new PointForceDirection(currentPath[i]->getLocation(s), - currentPath[i]->getParentFrame(), Vec3(0)); - rPFDs->append(pfd); - } - - for (i = 0; i < np-1; i++) { - start = currentPath[i]; - end = currentPath[i+1]; - startBody = &start->getParentFrame(); - endBody = &end->getParentFrame(); - - if (startBody != endBody) - { - Vec3 posStart, posEnd; - Vec3 direction(0); - - // Find the positions of start and end in the inertial frame. - //engine.getPosition(s, start->getParentFrame(), start->getLocation(), posStart); - posStart = start->getLocationInGround(s); - - //engine.getPosition(s, end->getParentFrame(), end->getLocation(), posEnd); - posEnd = end->getLocationInGround(s); - - // Form a vector from start to end, in the inertial frame. - direction = (posEnd - posStart); - - // Check that the two points are not coincident. - // This can happen due to infeasible wrapping of the path, - // when the origin or insertion enters the wrapping surface. - // This is a temporary fix, since the wrap algorithm should - // return NaN for the points and/or throw an Exception- aseth - if (direction.norm() < SimTK::SignificantReal){ - direction = direction*SimTK::NaN; - } - else{ - direction = direction.normalize(); - } - - // Get resultant direction at each point - rPFDs->get(i)->addToDirection(direction); - rPFDs->get(i+1)->addToDirection(-direction); - } - } -} - -void GeometryPath::addInEquivalentForces( - const SimTK::State& s, - 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); - } - } - } -} - -double GeometryPath::computeMomentArm( - const SimTK::State& s, - const Coordinate& aCoord) const -{ - if (!_maSolver) { - const_cast(this)->_maSolver.reset(new MomentArmSolver(*_model)); - } - - return _maSolver->solve(s, aCoord, *this); -} - - -void GeometryPath::extendPreScale(const SimTK::State& s, - const ScaleSet& scaleSet) -{ - Super::extendPreScale(s, scaleSet); - setPreScaleLength(s, getLength(s)); -} - -void GeometryPath::extendPostScale(const SimTK::State& s, - const ScaleSet& scaleSet) -{ - Super::extendPostScale(s, scaleSet); - computePath(s); -} - - -void GeometryPath::updateGeometry(const SimTK::State& s) const -{ - // Check if the current path needs to recomputed. - computePath(s); -} - - -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()); - } - } -} - -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.") - - // 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 GeometryPath::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 GeometryPath::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 GeometryPath::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]; - 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; - } - } -} - - -// private methods - - -void GeometryPath::constructProperties() -{ - constructProperty_PathPointSet(PathPointSet()); - constructProperty_PathWrapSet(PathWrapSet()); Appearance appearance; appearance.set_color(SimTK::Gray); constructProperty_Appearance(appearance); } - -void GeometryPath::computePath(const SimTK::State& s) const -{ - if (isCacheVariableValid(s, _currentPathCV)) { - return; - } - - // Clear the current path. - Array& currentPath = updCacheVariableValue(s, _currentPathCV); - currentPath.setSize(0); - - // Add the active fixed and moving via points to the path. - for (int i = 0; i < get_PathPointSet().getSize(); i++) { - if (get_PathPointSet()[i].isActive(s)) - currentPath.append(&get_PathPointSet()[i]); // <--- !!!!BAD - } - - // Use the current path so far to check for intersection with wrap objects, - // which may add additional points to the path. - applyWrapObjects(s, currentPath); - calcLengthAfterPathComputation(s, currentPath); - - markCacheVariableValid(s, _currentPathCV); -} - -void GeometryPath::computeLengtheningSpeed(const SimTK::State& s) const -{ - if (isCacheVariableValid(s, _speedCV)) { - return; - } - - const Array& currentPath = getCurrentPath(s); - - double speed = 0.0; - for (int i = 0; i < currentPath.getSize() - 1; i++) { - speed += currentPath[i]->calcSpeedBetween(s, *currentPath[i+1]); - } - - setLengtheningSpeed(s, speed); -} - -void GeometryPath::applyWrapObjects(const SimTK::State& s, - Array& path) const -{ - if (get_PathWrapSet().getSize() < 1) { - return; - } - - WrapResult best_wrap; - Array result, order; - - result.setSize(get_PathWrapSet().getSize()); - order.setSize(get_PathWrapSet().getSize()); - - // Set the initial order to be the order they are listed in the path. - for (int i = 0; i < get_PathWrapSet().getSize(); i++) - order[i] = i; - - // If there is only one wrap object, calculate the wrapping only once. - // If there are two or more objects, perform up to 8 iterations where - // the result from one wrap object is used as the starting point for - // the next wrap. - const int maxIterations = get_PathWrapSet().getSize() < 2 ? 1 : 8; - double last_length = SimTK::Infinity; - for (int kk = 0; kk < maxIterations; kk++) - { - for (int i = 0; i < get_PathWrapSet().getSize(); i++) - { - result[i] = 0; - PathWrap& ws = get_PathWrapSet().get(order[i]); - const WrapObject* wo = ws.getWrapObject(); - best_wrap.wrap_pts.setSize(0); - double min_length_change = SimTK::Infinity; - - // First remove this object's wrapping points from the current path. - for (int j = 0; j get_active()) { - // startPoint and endPoint in wrapStruct represent the - // user-defined starting and ending points in the array of path - // points that should be considered for wrapping. These indices - // take into account via points, whether or not they are active. - // Thus they are indices into mp_orig[], not mp[] (also, mp[] - // may contain wrapping points from previous wrap objects, which - // would mess up the starting and ending indices). But the goal - // is to find starting and ending indices in mp[] to consider - // for wrapping over this wrap object. Here is how that is done: - - // 1. startPoint and endPoint are 1-based, so subtract 1 from - // them to get indices into get_PathPointSet(). -1 (or any value - // less than 1) means use the first (or last) point. - const int wrapStart = (ws.getStartPoint() < 1 - ? 0 - : ws.getStartPoint() - 1); - const int wrapEnd = (ws.getEndPoint() < 1 - ? get_PathPointSet().getSize() - 1 - : ws.getEndPoint() - 1); - - // 2. Scan forward from wrapStart in get_PathPointSet() to find - // the first point that is active. Store a pointer to it (smp). - int jfwd = wrapStart; - for (; jfwd <= wrapEnd; jfwd++) - if (get_PathPointSet().get(jfwd).isActive(s)) - break; - if (jfwd > wrapEnd) // there are no active points in the path - return; - const AbstractPathPoint* const smp = &get_PathPointSet().get(jfwd); - - // 3. Scan backwards from wrapEnd in get_PathPointSet() to find - // the last point that is active. Store a pointer to it (emp). - int jrev = wrapEnd; - for (; jrev >= wrapStart; jrev--) - if (get_PathPointSet().get(jrev).isActive(s)) - break; - if (jrev < wrapStart) // there are no active points in the path - return; - const AbstractPathPoint* const emp = &get_PathPointSet().get(jrev); - - // 4. Now find the indices of smp and emp in _currentPath. - int start=-1, end=-1; - for (int j = 0; j < path.getSize(); j++) { - if (path.get(j) == smp) - start = j; - if (path.get(j) == emp) - end = j; - } - if (start == -1 || end == -1) // this should never happen - return; - - // You now have indices into _currentPath (which is a list of - // all currently active points, including wrap points) that - // represent the used-defined range of points to consider for - // wrapping over this wrap object. Check each path segment in - // this range, choosing the best wrap as the one that changes - // the path segment length the least: - for (int pt1 = start; pt1 < end; pt1++) - { - const int pt2 = pt1 + 1; - - // As long as the two points are not auto wrap points on the - // same wrap object, check them for wrapping. - if ( path.get(pt1)->getWrapObject() == NULL - || path.get(pt2)->getWrapObject() == NULL - || ( path.get(pt1)->getWrapObject() - != path.get(pt2)->getWrapObject())) - { - WrapResult wr; - wr.startPoint = pt1; - wr.endPoint = pt2; - - result[i] = wo->wrapPathSegment(s, *path.get(pt1), - *path.get(pt2), ws, wr); - if (result[i] == WrapObject::mandatoryWrap) { - // "mandatoryWrap" means the path actually - // intersected the wrap object. In this case, you - // *must* choose this segment as the "best" one for - // wrapping. If the path has more than one segment - // that intersects the object, the first one is - // taken as the mandatory wrap (this is considered - // an ill-conditioned case). - best_wrap = wr; - // Store the best wrap in the pathWrap for possible - // use next time. - ws.setPreviousWrap(wr); - break; - } else if (result[i] == WrapObject::wrapped) { - // "wrapped" means the path segment was wrapped over - // the object, but you should consider the other - // segments as well to see if one - // wraps with a smaller length change. - double path_length_change = - calcPathLengthChange(s, *wo, wr, path); - if (path_length_change < min_length_change) - { - best_wrap = wr; - // Store the best wrap in the pathWrap for - // possible use next time - ws.setPreviousWrap(wr); - min_length_change = path_length_change; - } else { - // The wrap was not shorter than the current - // minimum, so just free the wrap points that - // were allocated. - wr.wrap_pts.setSize(0); - } - } else { - // Nothing to do. - } - } - } - - // Deallocate previous wrapping points if necessary. - ws.updWrapPoint2().getWrapPath().setSize(0); - - if (best_wrap.wrap_pts.getSize() == 0) { - ws.resetPreviousWrap(); - ws.updWrapPoint2().getWrapPath().setSize(0); - } else { - // If wrapping did occur, copy wrap info into the PathStruct. - ws.updWrapPoint1().getWrapPath().setSize(0); - - Array& wrapPath = ws.updWrapPoint2().getWrapPath(); - wrapPath = best_wrap.wrap_pts; - - // In OpenSim, all conversion to/from the wrap object's - // reference frame will be performed inside - // wrapPathSegment(). Thus, all points in this function will - // be in their respective body reference frames. - // for (j = 0; j < wrapPath.getSize(); j++){ - // convert_from_wrap_object_frame(wo, wrapPath.get(j)); - // convert(ms->modelnum, wrapPath.get(j), wo->segment, - // ms->ground_segment); - // } - - ws.updWrapPoint1().setWrapLength(0.0); - ws.updWrapPoint2().setWrapLength(best_wrap.wrap_path_length); - - ws.updWrapPoint1().setLocation(best_wrap.r1); - ws.updWrapPoint2().setLocation(best_wrap.r2); - - // Now insert the two new wrapping points into mp[] array. - path.insert(best_wrap.endPoint, &ws.updWrapPoint1()); - path.insert(best_wrap.endPoint + 1, &ws.updWrapPoint2()); - } - } - } - - const double length = calcLengthAfterPathComputation(s, path); - if (std::abs(length - last_length) < 0.0005) { - break; - } else { - last_length = length; - } - - if (kk == 0 && get_PathWrapSet().getSize() > 1) { - // If the first wrap was a no wrap, and the second was a no wrap - // because a point was inside the object, switch the order of - // the first two objects and try again. - if ( result[0] == WrapObject::noWrap - && result[1] == WrapObject::insideRadius) - { - order[0] = 1; - order[1] = 0; - - // remove wrap object 0 from the list of path points - PathWrap& ws = get_PathWrapSet().get(0); - for (int j = 0; j < path.getSize(); j++) { - if (path.get(j) == &ws.updWrapPoint1()) { - path.remove(j); // remove the first wrap point - path.remove(j); // remove the second wrap point - break; - } - } - } - } - } -} - -/* - * _calc_path_length_change - given the output of a successful path wrap - * over a wrap object, determine the percent change in length of the - * path segment incurred by wrapping. - */ -double GeometryPath::calcPathLengthChange( - const SimTK::State& s, - const WrapObject& wo, - const WrapResult& wr, - const Array& path) const -{ - const AbstractPathPoint* pt1 = path.get(wr.startPoint); - const AbstractPathPoint* pt2 = path.get(wr.endPoint); - - double straight_length = pt1->calcDistanceBetween(s, *pt2); - - double wrap_length = pt1->calcDistanceBetween(s, wo.getFrame(), wr.r1); - wrap_length += wr.wrap_path_length; - wrap_length += pt2->calcDistanceBetween(s, wo.getFrame(), wr.r2); - - return wrap_length - straight_length; // return absolute diff, not relative -} - -/* - * Compute the total length of the path. This function - * assumes that the path has already been updated. - */ -double GeometryPath::calcLengthAfterPathComputation( - const SimTK::State& s, - const Array& currentPath) const -{ - double length = 0.0; - - for (int i = 0; i < currentPath.getSize() - 1; i++) { - const AbstractPathPoint* p1 = currentPath[i]; - const AbstractPathPoint* p2 = currentPath[i+1]; - - // If both points are wrap points on the same wrap object, then this - // path segment wraps over the surface of a wrap object, so just add in - // the pre-calculated length. - if ( p1->getWrapObject() - && p2->getWrapObject() - && p1->getWrapObject() == p2->getWrapObject()) - { - const PathWrapPoint* smwp = dynamic_cast(p2); - if (smwp) - length += smwp->getWrapLength(); - } else { - length += p1->calcDistanceBetween(s, *p2); - } - } - - setLength(s,length); - return( length ); -} - -/* - * Name the path points based on their position in the set. To keep the - * names up to date, this method should be called every time the path changes. - * - * @param aStartingIndex The index of the first path point to name. - */ -void GeometryPath::namePathPoints(int aStartingIndex) -{ - char indx[5]; - for (int i = aStartingIndex; i < get_PathPointSet().getSize(); i++) - { - sprintf(indx,"%d",i+1); - AbstractPathPoint& point = get_PathPointSet().get(i); - if (point.getName()=="" && hasOwner()) { - point.setName(getOwner().getName() + "-P" + indx); - } - } -} - -/* - * Determine an appropriate default XYZ location for a new path point. - * Note that this method is internal and should not be called directly on a new - * point as the point is not really added to the path (done in addPathPoint() - * instead) - * @param aOffset The XYZ location to be determined. - * @param aIndex The position in the pathPointSet to put the new point in. - * @param frame The body to attach the point to. - */ -void GeometryPath::placeNewPathPoint( - const SimTK::State& s, - SimTK::Vec3& aOffset, - int aIndex, - const PhysicalFrame& frame) -{ - // The location of the point is determined by moving a 'distance' from 'base' - // along a vector from 'start' to 'end.' 'base' is the existing path point - // that is in or closest to the index aIndex. 'start' and 'end' are existing - // path points--which ones depends on where the new point is being added. - // 'distance' is 0.5 for points added to the middle of a path (so the point - // appears halfway between the two adjacent points), and 0.2 for points that - // are added to either end of the path. If there is only one point in the - // path, the new point is put 0.01 units away in all three dimensions. - if (get_PathPointSet().getSize() > 1) { - int start, end, base; - double distance; - if (aIndex == 0) { - start = 1; - end = 0; - base = end; - distance = 0.2; - } else if (aIndex >= get_PathPointSet().getSize()) { - start = aIndex - 2; - end = aIndex - 1; - base = end; - distance = 0.2; - } else { - start = aIndex; - end = aIndex - 1; - base = start; - distance = 0.5; - } - - const Vec3 startPt = get_PathPointSet()[start].getLocation(s); - const Vec3 endPt = get_PathPointSet()[end].getLocation(s); - const Vec3 basePt = get_PathPointSet()[base].getLocation(s); - - Vec3 startPt2 = get_PathPointSet()[start].getParentFrame() - .findStationLocationInAnotherFrame(s, startPt, frame); - - Vec3 endPt2 = get_PathPointSet()[end].getParentFrame() - .findStationLocationInAnotherFrame(s, endPt, frame); - - aOffset = basePt + distance * (endPt2 - startPt2); - } else if (get_PathPointSet().getSize() == 1) { - aOffset= get_PathPointSet()[0].getLocation(s) + 0.01; - } - else { // first point, do nothing? - } -} - -void GeometryPath::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/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index 6e6cc1c8c1..1c9d0ddc0c 100644 --- a/OpenSim/Simulation/Model/GeometryPath.h +++ b/OpenSim/Simulation/Model/GeometryPath.h @@ -25,7 +25,7 @@ * -------------------------------------------------------------------------- */ #include -#include +#include #include #include #include @@ -39,7 +39,9 @@ namespace OpenSim { +class AbstractPathPoint; class Coordinate; +class PhysicalFrame; class PointForceDirection; class ScaleSet; class WrapResult; @@ -52,9 +54,17 @@ class WrapObject; * 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); + OpenSim_DECLARE_ABSTRACT_OBJECT(GeometryPath, ModelComponent); //============================================================================= // OUTPUTS @@ -76,34 +86,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); OpenSim_DECLARE_UNNAMED_PROPERTY(Appearance, "Default appearance attributes for this 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"); - - // 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; - - mutable CacheVariable _lengthCV; - mutable CacheVariable _speedCV; - mutable CacheVariable> _currentPathCV; - mutable CacheVariable _colorCV; - //============================================================================= // METHODS //============================================================================= public: GeometryPath(); - const PathPointSet& getPathPointSet() const; - PathPointSet& updPathPointSet(); + virtual const PathPointSet& getPathPointSet() const = 0; + virtual PathPointSet& updPathPointSet() = 0; /** * Add a new path point, with default location, to the path. @@ -112,14 +102,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * @param frame The frame to attach the point to. * @return Pointer to the newly created path point. */ - AbstractPathPoint* addPathPoint( + virtual AbstractPathPoint* addPathPoint( const SimTK::State& s, int index, - const PhysicalFrame& frame); - AbstractPathPoint* appendNewPathPoint( + const PhysicalFrame& frame) = 0; + virtual AbstractPathPoint* appendNewPathPoint( const std::string& proposedName, const PhysicalFrame& frame, - const SimTK::Vec3& locationOnFrame); + const SimTK::Vec3& locationOnFrame) = 0; /** * Returns true if a path point can be deleted. All paths must have at * least two active path points to define the path. @@ -127,14 +117,14 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * @param aIndex The index of the point to delete. * @return Whether or not the point can be deleted. */ - bool canDeletePathPoint(int index); + virtual bool canDeletePathPoint(int index) = 0; /** * Delete a path point. * * @param aIndex The index of the point to delete. * @return Whether or not the point was deleted. */ - bool deletePathPoint(const SimTK::State& s, int index); + virtual bool deletePathPoint(const SimTK::State& s, int index) = 0; /** * 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 @@ -143,46 +133,46 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * @param aOldPathPoint Path point to remove. * @param aNewPathPoint Path point to add. */ - bool replacePathPoint( + virtual bool replacePathPoint( const SimTK::State& s, AbstractPathPoint* oldPathPoint, - AbstractPathPoint* newPathPoint); + AbstractPathPoint* newPathPoint) = 0; /** * Get the current path of the path. * * @return The array of currently active path points. */ - const Array& getCurrentPath(const SimTK::State& s) const; + virtual const Array& getCurrentPath(const SimTK::State& s) const = 0; - const PathWrapSet& getWrapSet() const; - PathWrapSet& updWrapSet(); + virtual const PathWrapSet& getWrapSet() const = 0; + virtual PathWrapSet& updWrapSet() = 0; /** * Create a new wrap instance and add it to the set. * * @param aWrapObject The wrap object to use in the new wrap instance. */ - void addPathWrap(WrapObject& aWrapObject); + virtual void addPathWrap(WrapObject& aWrapObject) = 0; /** * 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 moveUpPathWrap(const SimTK::State& s, int index); + virtual void moveUpPathWrap(const SimTK::State& s, int index) = 0; /** * 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 moveDownPathWrap(const SimTK::State& s, int index); + virtual void moveDownPathWrap(const SimTK::State& s, int index) = 0; /** * Delete a wrap instance. * * @param aIndex The index of the wrap instance to delete. */ - void deletePathWrap(const SimTK::State& s, int index); + virtual void deletePathWrap(const SimTK::State& s, int index) = 0; /** @@ -192,7 +182,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * at the next extendAddToSystem() call. The actual color used to draw the * path will be taken from the cache variable, which may have changed. */ - const SimTK::Vec3& getDefaultColor() const; + virtual const SimTK::Vec3& getDefaultColor() const = 0; /** * Set the default color of the path. @@ -201,7 +191,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * the color cache variable. Otherwise %GeometryPath will choose its own * default, which varies depending on owner. */ - void setDefaultColor(const SimTK::Vec3& color); + virtual void setDefaultColor(const SimTK::Vec3& color) = 0; /** * Get the current color of the path. @@ -214,7 +204,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * * @see setDefaultColor() */ - SimTK::Vec3 getColor(const SimTK::State& s) const; + virtual SimTK::Vec3 getColor(const SimTK::State& s) const = 0; /** * Set the current color of the path. @@ -225,21 +215,21 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * the state realized to Stage::Dynamics. Therefore, 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; + virtual void setColor(const SimTK::State& s, const SimTK::Vec3& color) const = 0; - double getLength(const SimTK::State& s) const; - void setLength(const SimTK::State& s, double length) const; + virtual double getLength(const SimTK::State& s) const = 0; + virtual void setLength(const SimTK::State& s, double length) const = 0; /** * Compute the lengthening speed of the path. * * @return lengthening speed of the path. */ - double getLengtheningSpeed(const SimTK::State& s) const; - void setLengtheningSpeed(const SimTK::State& s, double speed) const; + virtual double getLengtheningSpeed(const SimTK::State& s) const = 0; + virtual void setLengtheningSpeed(const SimTK::State& s, double speed) const = 0; - double getPreScaleLength(const SimTK::State& s) const; - void setPreScaleLength(const SimTK::State& s, double preScaleLength); + virtual double getPreScaleLength(const SimTK::State& s) const = 0; + virtual void setPreScaleLength(const SimTK::State& s, double preScaleLength) = 0; /** * Appends PointForceDirections to the output parameter. @@ -248,8 +238,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * * CAUTION: the return pointers are heap allocated: you must delete them yourself! */ - void getPointForceDirections(const SimTK::State& s, - OpenSim::Array* rPFDs) const; + virtual void getPointForceDirections( + const SimTK::State& s, + OpenSim::Array* rPFDs) const = 0; /** * Add in the equivalent body and generalized forces to be applied to the @@ -260,10 +251,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * @param[in,out] bodyForces Vector of SpatialVec's (torque, force) on bodies * @param[in,out] mobilityForces Vector of generalized forces, one per mobility */ - void addInEquivalentForces(const SimTK::State& state, - double tension, - SimTK::Vector_& bodyForces, - SimTK::Vector& mobilityForces) const; + virtual void addInEquivalentForces(const SimTK::State& state, + double tension, + SimTK::Vector_& bodyForces, + SimTK::Vector& mobilityForces) const = 0; /** * Compute the path's moment arms for specified coordinate. @@ -271,68 +262,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(GeometryPath, ModelComponent); * @param aCoord, the coordinate */ 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; + const Coordinate& aCoord) const = 0; /** * Updates the geometry attached to the path (location of path points and * connecting segments all in global/inertial frame) */ - virtual void updateGeometry(const SimTK::State& s) const; - -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 constructProperties(); - - 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; + virtual void updateGeometry(const SimTK::State& s) const = 0; }; } 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/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..ab0ef8cd5c --- /dev/null +++ b/OpenSim/Simulation/Model/PointBasedPath.cpp @@ -0,0 +1,1092 @@ +/* -------------------------------------------------------------------------- * + * 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{}, + _preScaleLength{0.0}, + _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); +} + + +const SimTK::Vec3& OpenSim::PointBasedPath::getDefaultColor() const +{ + return get_Appearance().get_color(); +} + +void OpenSim::PointBasedPath::setDefaultColor(const SimTK::Vec3& color) +{ + updProperty_Appearance().setValueIsDefault(false); + upd_Appearance().set_color(color); +} + + +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); +} + + +double OpenSim::PointBasedPath::getPreScaleLength(const SimTK::State& s) const +{ + return _preScaleLength; +} + +void OpenSim::PointBasedPath::setPreScaleLength(const SimTK::State& s, double length) +{ + _preScaleLength = length; +} + +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..e5fe2569de --- /dev/null +++ b/OpenSim/Simulation/Model/PointBasedPath.h @@ -0,0 +1,177 @@ +#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"); + + // 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; + + 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; + + const SimTK::Vec3& getDefaultColor() const override; + void setDefaultColor(const SimTK::Vec3& color) 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; + + double getPreScaleLength(const SimTK::State& s) const override; + void setPreScaleLength(const SimTK::State& s, double preScaleLength) 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; + + + + //-------------------------------------------------------------------------- + // 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; + +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..6a617a9d9c 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -59,7 +59,7 @@ #include "Model/PathPointSet.h" #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" -#include "Model/GeometryPath.h" +#include "Model/PointBasedPath.h" #include "Model/PrescribedForce.h" #include "Model/ExternalForce.h" #include "Model/PointToPointSpring.h" @@ -188,7 +188,7 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( LineGeometry()); Object::registerType( FrameGeometry()); Object::registerType( Arrow()); - Object::registerType( GeometryPath()); + Object::registerType( OpenSim::PointBasedPath()); Object::registerType( ControlSet() ); Object::registerType( ControlConstant() ); @@ -307,6 +307,8 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::renameType("MuscleMetabolicPowerProbeUmberger2010_MetabolicMuscleParameterSet", "Umberger2010MuscleMetabolicsProbe_MetabolicMuscleParameterSet"); + Object::renameType("GeometryPath", "PointBasedPath"); + } catch (const std::exception& e) { std::cerr << "ERROR during osimSimulation Object registration:\n" diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 17b773ad80..59c05e24dc 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -53,6 +53,7 @@ #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" #include "Model/GeometryPath.h" +#include "Model/PointBasedPath.h" #include "Model/PrescribedForce.h" #include "Model/PointToPointSpring.h" #include "Model/ExpressionBasedPointToPointForce.h" From fa92353372365434278b94b12ec2b74c5c527be9 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 1 Dec 2021 12:44:06 +0100 Subject: [PATCH 03/12] Implemented deprecations, and deprecation stub methods --- OpenSim/Simulation/Model/GeometryPath.cpp | 246 +++++++++++++++- OpenSim/Simulation/Model/GeometryPath.h | 302 +++++++++++++------- OpenSim/Simulation/Model/PointBasedPath.cpp | 29 +- OpenSim/Simulation/Model/PointBasedPath.h | 15 - 4 files changed, 445 insertions(+), 147 deletions(-) diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 497f898971..2acd445af3 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -25,7 +25,76 @@ #include -OpenSim::GeometryPath::GeometryPath() : ModelComponent{} + +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; + +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; + + // used as a stand-in if the caller uses the (deprecated) point-based API + SimTK::ClonePtr fixupPathPointSet = nullptr; + + // used as a stand-in if the caller uses the (deprecated) point-based API + SimTK::ClonePtr fixupPathPoint = nullptr; + + // used as a stand-in if the caller uses the (deprecated) point-based API + OpenSim::Array fixupCurrentPathArray; + + // used as a stand-in if the caller uses the (deprecated) point-based API + SimTK::ClonePtr fixupPathWrapSet = nullptr; + +public: + Impl* clone() const { + return new Impl{*this}; + } + + 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; + } + + OpenSim::AbstractPathPoint* getOrUpdFixupPathPointCached() const + { + if (!fixupPathPoint) { + std::lock_guard guard{const_cast(*this).constCastMutex.updT()}; + const_cast(*this).fixupPathPoint = new OpenSim::PathPoint{}; + } + return const_cast(*this).fixupPathPoint.upd(); + } + + OpenSim::Array& getOrUpdCurrentPathArray() const + { + return const_cast(*this).fixupCurrentPathArray; + } + + PathWrapSet& getOrUpdPathWrapSet() const + { + if (!fixupPathWrapSet) { + std::lock_guard guard{const_cast(*this).constCastMutex.updT()}; + const_cast(*this).fixupPathWrapSet = new OpenSim::PathWrapSet{}; + } + + return *const_cast(*this).fixupPathWrapSet.upd(); + } +}; + +OpenSim::GeometryPath::GeometryPath() : ModelComponent{}, _impl{new Impl{}} { setAuthors("Peter Loan"); @@ -33,3 +102,178 @@ OpenSim::GeometryPath::GeometryPath() : ModelComponent{} appearance.set_color(SimTK::Gray); constructProperty_Appearance(appearance); } + +OpenSim::GeometryPath::GeometryPath(OpenSim::GeometryPath const&) = default; + +OpenSim::GeometryPath::GeometryPath(OpenSim::GeometryPath&&) noexcept = default; + +OpenSim::GeometryPath::~GeometryPath() noexcept = default; + +OpenSim::GeometryPath& OpenSim::GeometryPath::operator=(const GeometryPath&) = default; + +OpenSim::GeometryPath& OpenSim::GeometryPath::operator=(GeometryPath&&) noexcept = default; + + +// DEFAULTED METHODS + +const SimTK::Vec3& OpenSim::GeometryPath::getDefaultColor() const +{ + return get_Appearance().get_color(); +} + +void OpenSim::GeometryPath::setDefaultColor(const SimTK::Vec3& color) +{ + updProperty_Appearance().setValueIsDefault(false); + upd_Appearance().set_color(color); +} + +double OpenSim::GeometryPath::getPreScaleLength(const SimTK::State&) const +{ + return _impl->_preScaleLength; +} + +void OpenSim::GeometryPath::setPreScaleLength(const SimTK::State&, double preScaleLength) +{ + _impl->_preScaleLength = preScaleLength; +} + + +// 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) +{ + OpenSim::log_warn("deprecation warning: OpenSim tried to call '{}', which is now deprecated. This may result in unusual outputs", funcName); + return true; +} + +const OpenSim::PathPointSet& OpenSim::GeometryPath::getPathPointSet() const +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return _impl->getOrUpdPPSCached(); +} + +OpenSim::PathPointSet& OpenSim::GeometryPath::updPathPointSet() +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return _impl->getOrUpdPPSCached(); +} + +OpenSim::AbstractPathPoint* OpenSim::GeometryPath::addPathPoint( + const SimTK::State& s, + int index, + const PhysicalFrame& frame) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + + // 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(); +} + +OpenSim::AbstractPathPoint* OpenSim::GeometryPath::appendNewPathPoint( + const std::string& proposedName, + const PhysicalFrame& frame, + const SimTK::Vec3& locationOnFrame) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + + // 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(); +} + +bool OpenSim::GeometryPath::canDeletePathPoint(int index) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return false; +} + +bool OpenSim::GeometryPath::deletePathPoint(const SimTK::State& s, int index) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return false; +} + +bool OpenSim::GeometryPath::replacePathPoint( + const SimTK::State& s, + AbstractPathPoint* oldPathPoint, + AbstractPathPoint* newPathPoint) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return false; +} + +const OpenSim::Array& OpenSim::GeometryPath::getCurrentPath(const SimTK::State& s) const +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return _impl->getOrUpdCurrentPathArray(); +} + +const OpenSim::PathWrapSet& OpenSim::GeometryPath::getWrapSet() const +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return _impl->getOrUpdPathWrapSet(); +} + +OpenSim::PathWrapSet& OpenSim::GeometryPath::updWrapSet() +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return _impl->getOrUpdPathWrapSet(); +} + +void OpenSim::GeometryPath::addPathWrap(WrapObject &aWrapObject) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return; +} + +void OpenSim::GeometryPath::moveUpPathWrap(const SimTK::State& s, int index) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return; +} + +void OpenSim::GeometryPath::moveDownPathWrap(const SimTK::State& s, int index) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return; +} + +void OpenSim::GeometryPath::deletePathWrap(const SimTK::State& s, int index) +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return; +} + +void OpenSim::GeometryPath::setLength(const SimTK::State& s, double length) const +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return; +} + +void OpenSim::GeometryPath::setLengtheningSpeed(const SimTK::State& s, double speed) const +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return; +} + +void OpenSim::GeometryPath::updateGeometry(const SimTK::State& s) const +{ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + return; +} diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index 1c9d0ddc0c..c994a0bdf1 100644 --- a/OpenSim/Simulation/Model/GeometryPath.h +++ b/OpenSim/Simulation/Model/GeometryPath.h @@ -86,14 +86,166 @@ class OSIMSIMULATION_API GeometryPath : public ModelComponent { OpenSim_DECLARE_UNNAMED_PROPERTY(Appearance, "Default appearance attributes for this GeometryPath"); + class Impl; +private: + SimTK::ClonePtr _impl; + //============================================================================= // METHODS //============================================================================= public: GeometryPath(); + GeometryPath(const GeometryPath&); + GeometryPath(GeometryPath&&) noexcept; + ~GeometryPath() noexcept; + + GeometryPath& operator=(const GeometryPath&); + GeometryPath& operator=(GeometryPath&&) noexcept; + + + // 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; - virtual const PathPointSet& getPathPointSet() const = 0; - virtual PathPointSet& updPathPointSet() = 0; + /** + * 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 = 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. @@ -102,14 +254,18 @@ class OSIMSIMULATION_API GeometryPath : public ModelComponent { * @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) = 0; + 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) = 0; + 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. @@ -117,14 +273,18 @@ class OSIMSIMULATION_API GeometryPath : public ModelComponent { * @param aIndex The index of the point to delete. * @return Whether or not the point can be deleted. */ - virtual bool canDeletePathPoint(int index) = 0; + 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. */ - virtual bool deletePathPoint(const SimTK::State& s, int index) = 0; + 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 @@ -133,142 +293,78 @@ class OSIMSIMULATION_API GeometryPath : public ModelComponent { * @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) = 0; + AbstractPathPoint* newPathPoint); + /** * Get the current path of the path. * * @return The array of currently active path points. */ - virtual const Array& getCurrentPath(const SimTK::State& s) const = 0; + 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(); - virtual const PathWrapSet& getWrapSet() const = 0; - virtual PathWrapSet& updWrapSet() = 0; /** * Create a new wrap instance and add it to the set. * * @param aWrapObject The wrap object to use in the new wrap instance. */ - virtual void addPathWrap(WrapObject& aWrapObject) = 0; + 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. */ - virtual void moveUpPathWrap(const SimTK::State& s, int index) = 0; + 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. */ - virtual void moveDownPathWrap(const SimTK::State& s, int index) = 0; + 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. */ - virtual void deletePathWrap(const SimTK::State& s, int index) = 0; - - - /** - * Get the default color of the path. - * - * 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, which may have changed. - */ - virtual const SimTK::Vec3& getDefaultColor() const = 0; - - /** - * Set the default color of the path. - * - * 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. - */ - virtual void setDefaultColor(const SimTK::Vec3& color) = 0; - - /** - * Get the current color of the path. - * - * Internally, gets the 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() - */ - virtual SimTK::Vec3 getColor(const SimTK::State& s) const = 0; - - /** - * Set the current color of the path. - * - * Internally, sets 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. 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; + 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); - virtual double getLength(const SimTK::State& s) const = 0; - virtual void setLength(const SimTK::State& s, double length) const = 0; - /** - * Compute the lengthening speed of the path. - * - * @return lengthening speed of the path. - */ - virtual double getLengtheningSpeed(const SimTK::State& s) const = 0; - virtual void setLengtheningSpeed(const SimTK::State& s, double speed) const = 0; + 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; - virtual double getPreScaleLength(const SimTK::State& s) const = 0; - virtual void setPreScaleLength(const SimTK::State& s, double preScaleLength) = 0; + 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. + * Proactively updates any decorative geometry attached to the path. * - * CAUTION: the return pointers are heap allocated: you must delete them yourself! - */ - virtual void getPointForceDirections( - const SimTK::State& s, - OpenSim::Array* rPFDs) const = 0; - - /** - * Add in the equivalent body and generalized forces to be applied to the - * multibody system resulting from a tension along the GeometryPath. - * - * @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; - - /** - * Compute the path's moment arms for specified coordinate. - * - * @param aCoord, the coordinate - */ - virtual double computeMomentArm(const SimTK::State& s, - const Coordinate& aCoord) const = 0; - - /** - * Updates the geometry attached to the path (location of path points and - * connecting segments all in global/inertial frame) + * 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. */ - virtual void updateGeometry(const SimTK::State& s) const = 0; + 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; }; } diff --git a/OpenSim/Simulation/Model/PointBasedPath.cpp b/OpenSim/Simulation/Model/PointBasedPath.cpp index ab0ef8cd5c..7e4bfffa1e 100644 --- a/OpenSim/Simulation/Model/PointBasedPath.cpp +++ b/OpenSim/Simulation/Model/PointBasedPath.cpp @@ -29,10 +29,7 @@ #include #include "Model.h" -OpenSim::PointBasedPath::PointBasedPath() : - GeometryPath{}, - _preScaleLength{0.0}, - _maSolver{nullptr} +OpenSim::PointBasedPath::PointBasedPath() : GeometryPath{}, _maSolver{nullptr} { setAuthors("Peter Loan"); constructProperty_PathPointSet(PathPointSet()); @@ -242,19 +239,6 @@ void OpenSim::PointBasedPath::deletePathWrap(const SimTK::State& s, int aIndex) upd_PathWrapSet().remove(aIndex); } - -const SimTK::Vec3& OpenSim::PointBasedPath::getDefaultColor() const -{ - return get_Appearance().get_color(); -} - -void OpenSim::PointBasedPath::setDefaultColor(const SimTK::Vec3& color) -{ - updProperty_Appearance().setValueIsDefault(false); - upd_Appearance().set_color(color); -} - - SimTK::Vec3 OpenSim::PointBasedPath::getColor(const SimTK::State& s) const { return getCacheVariableValue(s, _colorCV); @@ -290,17 +274,6 @@ void OpenSim::PointBasedPath::setLengtheningSpeed(const SimTK::State& s, double setCacheVariableValue(s, _speedCV, speed); } - -double OpenSim::PointBasedPath::getPreScaleLength(const SimTK::State& s) const -{ - return _preScaleLength; -} - -void OpenSim::PointBasedPath::setPreScaleLength(const SimTK::State& s, double length) -{ - _preScaleLength = length; -} - void OpenSim::PointBasedPath::getPointForceDirections( const SimTK::State& s, OpenSim::Array *rPFDs) const diff --git a/OpenSim/Simulation/Model/PointBasedPath.h b/OpenSim/Simulation/Model/PointBasedPath.h index e5fe2569de..187cc3195d 100644 --- a/OpenSim/Simulation/Model/PointBasedPath.h +++ b/OpenSim/Simulation/Model/PointBasedPath.h @@ -48,9 +48,6 @@ class OSIMSIMULATION_API PointBasedPath final : public GeometryPath { 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. @@ -90,9 +87,6 @@ class OSIMSIMULATION_API PointBasedPath final : public GeometryPath { void moveDownPathWrap(const SimTK::State& s, int index) override; void deletePathWrap(const SimTK::State& s, int index) override; - const SimTK::Vec3& getDefaultColor() const override; - void setDefaultColor(const SimTK::Vec3& color) override; - SimTK::Vec3 getColor(const SimTK::State& s) const override; void setColor(const SimTK::State& s, const SimTK::Vec3& color) const override; @@ -102,9 +96,6 @@ class OSIMSIMULATION_API PointBasedPath final : public GeometryPath { double getLengtheningSpeed(const SimTK::State& s) const override; void setLengtheningSpeed(const SimTK::State& s, double speed) const override; - double getPreScaleLength(const SimTK::State& s) const override; - void setPreScaleLength(const SimTK::State& s, double preScaleLength) override; - void getPointForceDirections( const SimTK::State& s, OpenSim::Array* rPFDs) const override; @@ -118,12 +109,6 @@ class OSIMSIMULATION_API PointBasedPath final : public GeometryPath { const Coordinate& aCoord) const override; void updateGeometry(const SimTK::State& s) const override; - - - //-------------------------------------------------------------------------- - // SCALING - //-------------------------------------------------------------------------- - /** * Calculate the path length in the current body position and store it for * use after the Model has been scaled. From 34116ece178c2ecec1fc0687a9d3b329fc13c2f3 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 1 Dec 2021 15:24:03 +0100 Subject: [PATCH 04/12] Added FunctionBasedPath stub with basic (stub) test suite --- .../Simulation/Model/FunctionBasedPath.cpp | 56 +++++ OpenSim/Simulation/Model/FunctionBasedPath.h | 60 +++++ .../Simulation/Test/testFunctionBasedPath.cpp | 226 ++++++++++++++++++ 3 files changed, 342 insertions(+) create mode 100644 OpenSim/Simulation/Model/FunctionBasedPath.cpp create mode 100644 OpenSim/Simulation/Model/FunctionBasedPath.h create mode 100644 OpenSim/Simulation/Test/testFunctionBasedPath.cpp diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp new file mode 100644 index 0000000000..5c71af1a9c --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -0,0 +1,56 @@ +#include "FunctionBasedPath.h" + + +OpenSim::FunctionBasedPath::FunctionBasedPath() +{ + // TODO: property init etc. +} + +OpenSim::FunctionBasedPath::FunctionBasedPath(const FunctionBasedPath&) = default; + +OpenSim::FunctionBasedPath::FunctionBasedPath(FunctionBasedPath&&) noexcept = default; + +OpenSim::FunctionBasedPath::~FunctionBasedPath() noexcept = default; + +OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(const FunctionBasedPath&) = default; + +OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(FunctionBasedPath&&) noexcept = default; + +SimTK::Vec3 OpenSim::FunctionBasedPath::getColor(const SimTK::State& s) const +{ + // TODO: read from cache variable or similar + return SimTK::Vec3{}; +} + +void OpenSim::FunctionBasedPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const +{ + // TODO: save to a cache variable or similar +} + +double OpenSim::FunctionBasedPath::getLength(const SimTK::State& s) const +{ + // TODO: compute via function abstraction + return 0.0; +} + +double OpenSim::FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const +{ + // TODO: compute via function abstraction + return 0.0; +} + +void OpenSim::FunctionBasedPath::getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const +{ + // TODO +} + +void OpenSim::FunctionBasedPath::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const +{ + // TODO +} + +double OpenSim::FunctionBasedPath::computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const +{ + // TODO: compute via function abstraction + return 0.0; +} diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h new file mode 100644 index 0000000000..dd0a9285d2 --- /dev/null +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -0,0 +1,60 @@ +#ifndef OPENSIM_FUNCTIONBASED_PATH_H_ +#define OPENSIM_FUNCTIONBASED_PATH_H_ + +#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 functions to compute its state. + */ +class FunctionBasedPath final : public GeometryPath { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, GeometryPath); + +public: + FunctionBasedPath(); + FunctionBasedPath(const FunctionBasedPath&); + FunctionBasedPath(FunctionBasedPath&&) noexcept; + ~FunctionBasedPath() noexcept; + + FunctionBasedPath& operator=(FunctionBasedPath const&); + FunctionBasedPath& operator=(FunctionBasedPath&&) noexcept; + + 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 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; +}; +} + +#endif // FUNCTIONBASEDPATH_H diff --git a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp new file mode 100644 index 0000000000..05d419800d --- /dev/null +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -0,0 +1,226 @@ +#include +#include + +#include +#include +#include +#include + +// this is a poor-man's GoogleTest, but I still prefer it to the madness of +// having a massive `main()` containing many `try..catch` blocks +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()}; + } +} + + +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)}; +} + +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); +} + +OSIM_TEST(FunctionBasedPath, CanGetColorWithoutThrowing) +{ + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + 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{}; + 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{}; + fbp->setDefaultColor(randomColor); + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + fbp->setColor(s, randomColor); + + SimTK_TEST(fbp->getColor(s) == randomColor); +} + +OSIM_TEST(FunctionBasedPath, CanCallGetLengthWithoutThrowing) +{ + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + fbp->getLength(s); // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, CanCallGetLengtheningSpeedWithoutThrowing) +{ + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + fbp->getLengtheningSpeed(s); // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, CanCallGetPointForceDirectionWithoutThrowing) +{ + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + OpenSim::Array pfds; + fbp->getPointForceDirections(s, &pfds); // shouldn't throw +} + +OSIM_TEST(FunctionBasedPath, CanCallAddInEquivalentForcesWithoutThrowing) +{ + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + model.addComponent(fbp); + + SimTK::State& s = model.initSystem(); + + SimTK::Vector_ bodyForces; + SimTK::Vector mobilityForces; + + fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); +} + +OSIM_TEST(FunctionBasedPath, CanCallComputeMomentArmWithoutThrowing) +{ + OpenSim::Model model; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + model.addComponent(fbp); + + OpenSim::Coordinate* coord = new OpenSim::Coordinate{}; + model.addComponent(coord); + + SimTK::State& s = model.initSystem(); + + SimTK::Vector_ bodyForces; + SimTK::Vector mobilityForces; + + fbp->computeMomentArm(s, *coord); +} + +int main() +{ + return RunAllTests(); +} From 5f84d1c36249f73c5e0fe123dc980b1dec14a95f Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 1 Dec 2021 16:24:43 +0100 Subject: [PATCH 05/12] Sketched PathFunction interface and added basic plumbing members --- .../Simulation/Model/FunctionBasedPath.cpp | 46 ++++++++++++++++++- OpenSim/Simulation/Model/FunctionBasedPath.h | 34 +++++++++++++- .../Simulation/Test/testFunctionBasedPath.cpp | 16 ++++--- 3 files changed, 87 insertions(+), 9 deletions(-) diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index 5c71af1a9c..2eab91bb02 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -1,9 +1,36 @@ #include "FunctionBasedPath.h" +namespace { + // used as a stub PathFunction implementation that throws if the user tries + // to actually use it + // + // handy for handling the case where we need *some* PathFunction to stand-in + // before (e.g.) a "proper" path function is assigned to a property or + // something. + static char const g_ThrowingPathErrMsg[] = "Cannot call any member on a ThrowingPathFunction. This exception has *probably* been throwng because a PathFunction has not (yet) been assigned for the parent component. You *probably* need to ensure that a FunctionBasedPath actually has PathFunction assigned to it"; + class ThrowingPathFunction final : public OpenSim::PathFunction { + OpenSim_DECLARE_CONCRETE_OBJECT(ThrowingPathFunction, OpenSim::PathFunction) -OpenSim::FunctionBasedPath::FunctionBasedPath() + double getLength(const SimTK::State&) override + { + OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + } + + double getLengtheningSpeed(const SimTK::State&) override + { + OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + } + + double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) override + { + OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + } + }; +} + +OpenSim::FunctionBasedPath::FunctionBasedPath() : GeometryPath{} { - // TODO: property init etc. + constructProperty_PathFunction(ThrowingPathFunction{}); } OpenSim::FunctionBasedPath::FunctionBasedPath(const FunctionBasedPath&) = default; @@ -54,3 +81,18 @@ double OpenSim::FunctionBasedPath::computeMomentArm(const SimTK::State& s, const // TODO: compute via function abstraction return 0.0; } + +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); +} diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h index dd0a9285d2..43b5f096d7 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.h +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -23,11 +23,41 @@ class Coordinate; class PointForceDirection; /** - * An `OpenSim::GeometryPath` that uses functions to compute its state. + * An interface for an object that can compute the length, lengthening speed, + * and moment arm (w.r.t. a particular `OpenSim::Coordinate`) of a path at + * runtime. + * + * See `FunctionBasedPath` for a standard implementation of a `GeometryPath` + * that automatically handles forwarding calls to the `PathFunction`, state + * caching, etc. + */ +class PathFunction : public OpenSim::Component { + OpenSim_DECLARE_ABSTRACT_OBJECT(PathFunction, OpenSim::Component) + +public: + virtual ~PathFunction() noexcept = default; + + virtual double getLength(const SimTK::State&) = 0; + virtual double getLengtheningSpeed(const SimTK::State&) = 0; + virtual double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) = 0; +}; + +/** + * 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 FunctionBasedPath final : public GeometryPath { OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, GeometryPath); + OpenSim_DECLARE_PROPERTY(PathFunction, PathFunction, "The underlying function that is used at simulation-time to evaluate the length, lengthening speed, and moment arm of the path."); + + mutable CacheVariable _lengthCV; + mutable CacheVariable _speedCV; + mutable CacheVariable _colorCV; + public: FunctionBasedPath(); FunctionBasedPath(const FunctionBasedPath&); @@ -54,6 +84,8 @@ class FunctionBasedPath final : public GeometryPath { double computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const override; + + void extendAddToSystem(SimTK::MultibodySystem& system) const override; }; } diff --git a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp index 05d419800d..39305f12a6 100644 --- a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -209,15 +210,18 @@ OSIM_TEST(FunctionBasedPath, CanCallComputeMomentArmWithoutThrowing) OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; model.addComponent(fbp); - OpenSim::Coordinate* coord = new OpenSim::Coordinate{}; - model.addComponent(coord); + OpenSim::Body* body = new OpenSim::Body{}; + body->setMass(1.0); + model.addComponent(body); - SimTK::State& s = model.initSystem(); + OpenSim::PinJoint* joint = new OpenSim::PinJoint{}; + joint->connectSocket_parent_frame(model.getGround()); + joint->connectSocket_child_frame(*body); + model.addJoint(joint); - SimTK::Vector_ bodyForces; - SimTK::Vector mobilityForces; + SimTK::State& s = model.initSystem(); - fbp->computeMomentArm(s, *coord); + fbp->computeMomentArm(s, joint->getCoordinate()); } int main() From d669e8dfbcf9535cac4b975c57e02fb0293f8865 Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 1 Dec 2021 17:55:07 +0100 Subject: [PATCH 06/12] Implemented + tested FunctionBasedPath forwarding to the PathFunction impl --- Bindings/OpenSimHeaders_simulation.h | 1 + .../Simulation/Model/FunctionBasedPath.cpp | 61 ++- OpenSim/Simulation/Model/FunctionBasedPath.h | 13 +- .../RegisterTypes_osimSimulation.cpp | 7 +- .../Simulation/Test/testFunctionBasedPath.cpp | 383 +++++++++++++++++- OpenSim/Simulation/osimSimulation.h | 1 + 6 files changed, 440 insertions(+), 26 deletions(-) diff --git a/Bindings/OpenSimHeaders_simulation.h b/Bindings/OpenSimHeaders_simulation.h index d3c5580ae5..7a9870562d 100644 --- a/Bindings/OpenSimHeaders_simulation.h +++ b/Bindings/OpenSimHeaders_simulation.h @@ -111,6 +111,7 @@ #include #include #include +#include #include #include diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index 2eab91bb02..46d40bae80 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -1,5 +1,9 @@ #include "FunctionBasedPath.h" +#include +#include +#include + namespace { // used as a stub PathFunction implementation that throws if the user tries // to actually use it @@ -11,17 +15,27 @@ namespace { class ThrowingPathFunction final : public OpenSim::PathFunction { OpenSim_DECLARE_CONCRETE_OBJECT(ThrowingPathFunction, OpenSim::PathFunction) - double getLength(const SimTK::State&) override + double getLength(const SimTK::State&) const override + { + OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + } + + double getLengtheningSpeed(const SimTK::State&) const override + { + OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + } + + double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const override { OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); } - double getLengtheningSpeed(const SimTK::State&) override + void getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const override { OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); } - double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) override + void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const override { OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); } @@ -37,6 +51,11 @@ OpenSim::FunctionBasedPath::FunctionBasedPath(const FunctionBasedPath&) = defaul OpenSim::FunctionBasedPath::FunctionBasedPath(FunctionBasedPath&&) noexcept = default; +OpenSim::FunctionBasedPath::FunctionBasedPath(PathFunction const& pathFn) : GeometryPath{} +{ + constructProperty_PathFunction(pathFn); +} + OpenSim::FunctionBasedPath::~FunctionBasedPath() noexcept = default; OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(const FunctionBasedPath&) = default; @@ -45,41 +64,49 @@ OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(FunctionBasedP SimTK::Vec3 OpenSim::FunctionBasedPath::getColor(const SimTK::State& s) const { - // TODO: read from cache variable or similar - return SimTK::Vec3{}; + return getCacheVariableValue(s, _colorCV); } void OpenSim::FunctionBasedPath::setColor(const SimTK::State& s, const SimTK::Vec3& color) const { - // TODO: save to a cache variable or similar + setCacheVariableValue(s, _colorCV, color); } double OpenSim::FunctionBasedPath::getLength(const SimTK::State& s) const { - // TODO: compute via function abstraction - return 0.0; + if (isCacheVariableValid(s, _lengthCV)) { + return getCacheVariableValue(s, _lengthCV); + } + + double v = getProperty_PathFunction().getValue().getLength(s); + setCacheVariableValue(s, _lengthCV, v); + return v; } double OpenSim::FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) const { - // TODO: compute via function abstraction - return 0.0; + if (isCacheVariableValid(s, _speedCV)) { + return getCacheVariableValue(s, _speedCV); + } + + double v = getProperty_PathFunction().getValue().getLengtheningSpeed(s); + setCacheVariableValue(s, _speedCV, v); + return v; } void OpenSim::FunctionBasedPath::getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const { - // TODO + getProperty_PathFunction().getValue().getPointForceDirections(s, rPFDs); } void OpenSim::FunctionBasedPath::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const { - // TODO + getProperty_PathFunction().getValue().addInEquivalentForces(state, tension, bodyForces, mobilityForces); } double OpenSim::FunctionBasedPath::computeMomentArm(const SimTK::State& s, const Coordinate& aCoord) const { - // TODO: compute via function abstraction - return 0.0; + return getProperty_PathFunction().getValue().computeMomentArm(s, aCoord); } void OpenSim::FunctionBasedPath::extendAddToSystem(SimTK::MultibodySystem& system) const @@ -96,3 +123,9 @@ void OpenSim::FunctionBasedPath::extendAddToSystem(SimTK::MultibodySystem& syste // 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); +} diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h index 43b5f096d7..534e3e5e8c 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.h +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -31,15 +32,17 @@ class PointForceDirection; * that automatically handles forwarding calls to the `PathFunction`, state * caching, etc. */ -class PathFunction : public OpenSim::Component { +class PathFunction : public OpenSim::ModelComponent { OpenSim_DECLARE_ABSTRACT_OBJECT(PathFunction, OpenSim::Component) public: virtual ~PathFunction() noexcept = default; - virtual double getLength(const SimTK::State&) = 0; - virtual double getLengtheningSpeed(const SimTK::State&) = 0; - virtual double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) = 0; + virtual double getLength(const SimTK::State&) const = 0; + virtual double getLengtheningSpeed(const SimTK::State&) const = 0; + virtual double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const = 0; + virtual void getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const = 0; + virtual void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const = 0; }; /** @@ -62,6 +65,7 @@ class FunctionBasedPath final : public GeometryPath { FunctionBasedPath(); FunctionBasedPath(const FunctionBasedPath&); FunctionBasedPath(FunctionBasedPath&&) noexcept; + explicit FunctionBasedPath(PathFunction const&); ~FunctionBasedPath() noexcept; FunctionBasedPath& operator=(FunctionBasedPath const&); @@ -86,6 +90,7 @@ class FunctionBasedPath final : public GeometryPath { const Coordinate& aCoord) const override; void extendAddToSystem(SimTK::MultibodySystem& system) const override; + void extendInitStateFromProperties(SimTK::State& s) const override; }; } diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 6a617a9d9c..73006f3313 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -60,6 +60,7 @@ #include "Model/ConditionalPathPoint.h" #include "Model/MovingPathPoint.h" #include "Model/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( OpenSim::PointBasedPath()); + 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"); @@ -307,8 +310,6 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::renameType("MuscleMetabolicPowerProbeUmberger2010_MetabolicMuscleParameterSet", "Umberger2010MuscleMetabolicsProbe_MetabolicMuscleParameterSet"); - Object::renameType("GeometryPath", "PointBasedPath"); - } catch (const std::exception& e) { std::cerr << "ERROR during osimSimulation Object registration:\n" diff --git a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp index 39305f12a6..aa6c76762c 100644 --- a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -81,6 +81,87 @@ namespace { { return SimTK::Vec3{GenerateDouble(), GenerateDouble(), GenerateDouble()}; } + + struct MockPathFunctionData { + int numTimesGetLengthCalled = 0; + double lengthValue = GenerateDouble(); + + int numTimesLengtheningSpeedCalled = 0; + double lengtheningSpeedValue = GenerateDouble(); + + int numTimesComputeMomentArmCalled = 0; + double computeMomentArmValue = GenerateDouble(); + + int numTimesGetPointForceDirectionsCalled = 0; + int numTimesAddInEquivalentForcesCalled = 0; + + int numTimesExtendConnectToModelCalled = 0; + int numTimesExtendInitStateFromPropertiesCalled = 0; + int numTimesExtendAddToSystemCalled = 0; + int numTimesExtendFinalizeFromPropertiesCalled = 0; + }; + + // used to test that the FunctionBasedPath is forwarding things correctly + class MockPathFunction : public OpenSim::PathFunction { + OpenSim_DECLARE_CONCRETE_OBJECT(MockPathFunction, OpenSim::PathFunction); + public: + mutable std::shared_ptr data = std::make_shared(); + + double getLength(const SimTK::State&) const override + { + ++data->numTimesGetLengthCalled; + return data->lengthValue; + } + + double getLengtheningSpeed(const SimTK::State&) const override + { + ++data->numTimesLengtheningSpeedCalled; + return data->lengtheningSpeedValue; + } + + double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const override + { + ++data->numTimesComputeMomentArmCalled; + return data->computeMomentArmValue; + } + + void getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const override + { + ++data->numTimesGetPointForceDirectionsCalled; + } + + void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const override + { + ++data->numTimesAddInEquivalentForcesCalled; + } + + // these are redundantly checked (the Component/Property tests *should* + // also test these) because existing implementations do use these methods + // to hook into the model/system/state at various steps (e.g. to cache + // coordinates, or whatever they need) and it's handy to redundantly + // ensure these are hooked up to the PathFunction via the FunctionBasedPath + // correctly. + + void extendConnectToModel(OpenSim::Model&) override + { + ++data->numTimesExtendConnectToModelCalled; + } + + void extendInitStateFromProperties(SimTK::State&) const override + { + ++data->numTimesExtendInitStateFromPropertiesCalled; + } + + void extendAddToSystem(SimTK::MultibodySystem&) const override + { + ++data->numTimesExtendAddToSystemCalled; + } + + void extendFinalizeFromProperties() override + { + ++data->numTimesExtendFinalizeFromPropertiesCalled; + } + }; } @@ -89,6 +170,30 @@ OSIM_TEST(FunctionBasedPath, CanBeDefaultConstructedWithoutThrowing) OpenSim::FunctionBasedPath fbp; // shouldn't throw } +OSIM_TEST(FunctionBasedPath, CanBeConstructedWithAPathFunction) +{ + MockPathFunction pathFn; + OpenSim::FunctionBasedPath fbp{pathFn}; +} + +OSIM_TEST(FunctionBasedPath, WhenConstructedWithPathFunctionUsesTheFunctionInLengthEvaluation) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + + SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); +} + OSIM_TEST(FunctionBasedPath, CanBeCopyConstructedWithoutThrowing) { OpenSim::FunctionBasedPath fbp1; @@ -159,29 +264,118 @@ OSIM_TEST(FunctionBasedPath, SetColorSetsTheColorInTheState) OSIM_TEST(FunctionBasedPath, CanCallGetLengthWithoutThrowing) { OpenSim::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; model.addComponent(fbp); SimTK::State& s = model.initSystem(); + model.realizePosition(s); fbp->getLength(s); // shouldn't throw } +OSIM_TEST(FunctionBasedPath, GetLengthUsesPathFunctionImpl) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + + SimTK::State& s = model.initSystem(); + model.realizePosition(s); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + + SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, GetLengthIsCached) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + + SimTK::State& s = model.initSystem(); + model.realizePosition(s); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + + SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); + + SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); // should cache + + SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); +} + OSIM_TEST(FunctionBasedPath, CanCallGetLengtheningSpeedWithoutThrowing) { OpenSim::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; model.addComponent(fbp); SimTK::State& s = model.initSystem(); + model.realizeAcceleration(s); fbp->getLengtheningSpeed(s); // shouldn't throw } +OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedUsesPathFunctionImpl) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); + + SimTK::State& s = model.initSystem(); + model.realizeAcceleration(s); + + SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); + + SimTK_TEST(fbp->getLengtheningSpeed(s) == pathFn.data->lengtheningSpeedValue); + + SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedIsCached) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); + + SimTK::State& s = model.initSystem(); + model.realizeAcceleration(s); + + SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); + + SimTK_TEST(fbp->getLengtheningSpeed(s) == pathFn.data->lengtheningSpeedValue); + + SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 1); + + SimTK_TEST(fbp->getLengtheningSpeed(s) == pathFn.data->lengtheningSpeedValue); // should cache + + SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 1); +} + OSIM_TEST(FunctionBasedPath, CanCallGetPointForceDirectionWithoutThrowing) { OpenSim::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; model.addComponent(fbp); SimTK::State& s = model.initSystem(); @@ -190,10 +384,60 @@ OSIM_TEST(FunctionBasedPath, CanCallGetPointForceDirectionWithoutThrowing) fbp->getPointForceDirections(s, &pfds); // shouldn't throw } +OSIM_TEST(FunctionBasedPath, GetPointForceDirectionUsesPathFunctionImpl) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); + + { + OpenSim::Array pfds; + fbp->getPointForceDirections(s, &pfds); + } + + SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, GetPointForceDirectionsIsNotCached) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); + + { + OpenSim::Array pfds; + fbp->getPointForceDirections(s, &pfds); + } + + SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 1); + + { + OpenSim::Array pfds; + fbp->getPointForceDirections(s, &pfds); // not cached + } + + SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 2); +} + OSIM_TEST(FunctionBasedPath, CanCallAddInEquivalentForcesWithoutThrowing) { OpenSim::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; model.addComponent(fbp); SimTK::State& s = model.initSystem(); @@ -204,10 +448,64 @@ OSIM_TEST(FunctionBasedPath, CanCallAddInEquivalentForcesWithoutThrowing) fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); } +OSIM_TEST(FunctionBasedPath, AddInEquivalentForcesUsesPathFunctionImpl) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + + { + SimTK::Vector_ bodyForces; + SimTK::Vector mobilityForces; + fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); + } + + SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, AddInEquivalentForcesIsNotCached) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + + { + SimTK::Vector_ bodyForces; + SimTK::Vector mobilityForces; + fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); + } + + SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 1); + + { + SimTK::Vector_ bodyForces; + SimTK::Vector mobilityForces; + fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); + } + + SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 2); +} + + OSIM_TEST(FunctionBasedPath, CanCallComputeMomentArmWithoutThrowing) { OpenSim::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; model.addComponent(fbp); OpenSim::Body* body = new OpenSim::Body{}; @@ -224,6 +522,81 @@ OSIM_TEST(FunctionBasedPath, CanCallComputeMomentArmWithoutThrowing) fbp->computeMomentArm(s, joint->getCoordinate()); } +OSIM_TEST(FunctionBasedPath, ComputeMomentArmUsesPathFunctionImpl) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + OpenSim::Body* body = new OpenSim::Body{}; + body->setMass(1.0); + model.addComponent(body); + + OpenSim::PinJoint* joint = new OpenSim::PinJoint{}; + joint->connectSocket_parent_frame(model.getGround()); + joint->connectSocket_child_frame(*body); + model.addJoint(joint); + + SimTK_TEST(pathFn.data->numTimesComputeMomentArmCalled == 0); + + SimTK::State& s = model.initSystem(); + + SimTK_TEST(pathFn.data->numTimesComputeMomentArmCalled == 0); + + SimTK_TEST(fbp->computeMomentArm(s, joint->getCoordinate()) == pathFn.data->computeMomentArmValue); + + SimTK_TEST(pathFn.data->numTimesComputeMomentArmCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, PathFunctionExtendConnectToModelIsCalledWhenFinalizeConnectionCalledOnTopLevelModel) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesExtendConnectToModelCalled == 0); + model.finalizeConnections(); + SimTK_TEST(pathFn.data->numTimesExtendConnectToModelCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, PathFunctionExtendInitStateFromPropertiesCalledWhenCalledOnTopLevelModel) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesExtendInitStateFromPropertiesCalled == 0); + model.initSystem(); + SimTK_TEST(pathFn.data->numTimesExtendConnectToModelCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, PathFunctionExtendAddToSystemCalledWhenInitSystemCalledOnTopLevelModel) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); + + SimTK_TEST(pathFn.data->numTimesExtendAddToSystemCalled == 0); + model.initSystem(); + SimTK_TEST(pathFn.data->numTimesExtendAddToSystemCalled == 1); +} + +OSIM_TEST(FunctionBasedPath, PathFunctionExtendFinalizePropertiesCalledWhenFinalizeFromPropertiesCalledOnTopLevelModel) +{ + OpenSim::Model model; + MockPathFunction pathFn; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + model.addComponent(fbp); // also calls it? + + SimTK_TEST(pathFn.data->numTimesExtendFinalizeFromPropertiesCalled == 1); + model.finalizeFromProperties(); + SimTK_TEST(pathFn.data->numTimesExtendFinalizeFromPropertiesCalled == 2); +} + int main() { return RunAllTests(); diff --git a/OpenSim/Simulation/osimSimulation.h b/OpenSim/Simulation/osimSimulation.h index 59c05e24dc..ae96d25d75 100644 --- a/OpenSim/Simulation/osimSimulation.h +++ b/OpenSim/Simulation/osimSimulation.h @@ -54,6 +54,7 @@ #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" From 3c25207dc5d36c063fdda539ffbf0a79cbaa308a Mon Sep 17 00:00:00 2001 From: Travis CI Date: Wed, 1 Dec 2021 19:28:53 +0100 Subject: [PATCH 07/12] Ported Joris's FunctionBasedPath implementation code into the testFunctionBasedPath unit --- OpenSim/Simulation/Model/ModelComponent.h | 2 +- .../Simulation/Test/testFunctionBasedPath.cpp | 908 ++++++++++++++++++ 2 files changed, 909 insertions(+), 1 deletion(-) 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/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp index aa6c76762c..9e613e831d 100644 --- a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -1,4 +1,5 @@ #include + #include #include @@ -597,6 +598,913 @@ OSIM_TEST(FunctionBasedPath, PathFunctionExtendFinalizePropertiesCalledWhenFinal SimTK_TEST(pathFn.data->numTimesExtendFinalizeFromPropertiesCalled == 2); } + + +// HACK: test Joris's implementation here +// +// this is because I cba'd splitting OSIM_TEST into another compilation unit, and because +// it's currently handy to have all this code in one unit while we flesh out the implementation + +#include + +namespace joris { + static constexpr size_t g_MaxCoordsThatCanBeInterpolated = 8; // important: this is an upper limit that's used for stack allocations + static constexpr int g_MaxCoordsThatCanAffectPathDefault = static_cast(g_MaxCoordsThatCanBeInterpolated); + static constexpr int g_NumProbingDiscretizationsDefault = 8; + static constexpr double g_MinProbingMomentArmChangeDefault = 0.001; + static constexpr int g_NumDiscretizationStepsPerDimensionDefault = 8; + + // returns `true` if changing the supplied `Coordinate` changes the moment arm + // of the supplied `PointBasedPath` (PBP) + 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; + } + + // discretization of a particular coordinate + // + // assumes `nsteps` evenly-spaced points ranging from [begin, end] (inclusive) + struct Discretization final { + double begin; + double end; + int nsteps; + }; + + // compute ideal discretization of the given coordinate + Discretization discretizationForCoord(OpenSim::Coordinate const& c, int numDiscretizationSteps) + { + SimTK_ASSERT_ALWAYS(numDiscretizationSteps >= 4, "need to supply more than 4 discretization steps"); + + Discretization d; + //d.begin = -static_cast(SimTK_PI)/2; + //d.end = static_cast(SimTK_PI)/2; + d.begin = std::max(c.getRangeMin(), -static_cast(SimTK_PI)); + d.end = std::min(c.getRangeMax(), static_cast(SimTK_PI)); + d.nsteps = numDiscretizationSteps - 3; + double step = (d.end-d.begin) / (d.nsteps-1); + + // expand range slightly in either direction to ensure interpolation is + // clean around the edges + d.begin -= step; + d.end += 2.0 * step; + d.nsteps += 3; + + return d; + } + + // compute all permutations of the coordinates for the given discretizations + // + // these output values are stored "lexographically", with coords being "big endian", so: + // + // - [coord[0].begin, coord[1].begin, ..., coord[n-1].begin] + // - [coord[0].begin, coord[1].begin, ..., (coord[n-1].begin + step)] + // - [coord[0].begin, coord[1].begin, ..., (coord[n-1].begin + 2*step)] + // - ... + // - [coord[0].begin, (coord[1].begin + step), ..., coord[n-1].begin] + // - [coord[0].begin, (coord[1].begin + step), ..., (coord[n-1].begin + step)] + // - ... + // - [(coord[0].begin + step), coord[1].begin, ..., coord[n-1].begin] + // - [(coord[0].begin + step), coord[1].begin, ..., (coord[n-1].begin + step)] + std::vector computeEvaluationsFromPBP( + OpenSim::PointBasedPath const& pbp, + SimTK::State& state, + OpenSim::Coordinate const** coords, + Discretization const* discs, + size_t ncoords) + { + std::vector rv; + + if (ncoords == 0) { // edge-case: logic below assumes ncoords > 0 + return rv; + } + + OPENSIM_THROW_IF(ncoords > g_MaxCoordsThatCanBeInterpolated, OpenSim::Exception, "too many coordinates affect this path - the FunctionBasedPath implementation cannot handle this"); + + // number of evaluations is the total number of permutations of all dimensions for + // all discretizations + int expectedEvals = 1; + for (size_t i = 0; i < ncoords; ++i) { + expectedEvals *= discs[i].nsteps; + } + rv.reserve(expectedEvals); + + // holds which "step" in each Coordinate's [begin, end] discretization we + // have evaluated up to + std::array discStepIdx{}; + while (discStepIdx[0] < discs[0].nsteps) { + + // set all coordinate values for this step + for (size_t coord = 0; coord < ncoords; ++coord) { + Discretization const& discr = discs[coord]; + + double stepSz = (discr.end - discr.begin) / (discr.nsteps - 1); + int step = discStepIdx[coord]; + double val = discr.begin + step*stepSz; + + coords[coord]->setValue(state, val); + } + + // eval the length of the PBP for this permutation of coordinate values + { + double eval = pbp.getLength(state); + rv.push_back(eval); + } + + // update which coordinate steps we're up to for each coordinate + // + // always updates "least significant" coordinate first, then performs + // "carry propagation" to the "more significant" coordinates + int pos = ncoords - 1; + discStepIdx[pos]++; + while (pos > 0 && discStepIdx[pos] >= discs[pos].nsteps) { + discStepIdx[pos] = 0; // overflow + ++discStepIdx[pos-1]; // carry + --pos; + } + } + + SimTK_ASSERT_ALWAYS(discStepIdx[0] == discs[0].nsteps, "should be true, after the final overflow"); + for (size_t i = 1; i < discStepIdx.size(); ++i) { + SimTK_ASSERT_ALWAYS(discStepIdx[i] == 0, "these less-significant coordinates should all be overflow-n by the end of the alg"); + } + SimTK_ASSERT_ALWAYS(rv.size() == static_cast(expectedEvals), "these two values should match, given the above alg"); + + return rv; + } + + 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 JorisFBP : public OpenSim::PathFunction { + OpenSim_DECLARE_CONCRETE_OBJECT(JorisFBP, OpenSim::PathFunction); + + public: + OpenSim_DECLARE_UNNAMED_PROPERTY(FunctionBasedPathDiscretizationSet, "Discretizations that were used for each OpenSim::Coordinate that the path was fitted against"); + OpenSim_DECLARE_LIST_PROPERTY(Evaluations, double, "The evaluated results of each *permutation* of discretizations. The FunctionBasedPathDiscretizationSet property describes how each OpenSim::Coordinate was discretized. These evaluations are the result of permuting through all possible combinations of discretizations. Effectively, this property contains a N-dimensional 'surface' of points, where each dimension of the surface is a Coordinate, and each dimension of each point is one of the evenly-spaced points in the discretization range [x_begin, x_range] for each dimension"); + + // direct pointers to each coordinate + std::vector coords; + + // absolute paths of each coordinate (1:1 with coords) + std::vector coordAbsPaths; + + // discretizations ranges for each coordinate (1:1 with coords) + std::vector discretizations; + + // evaluations for each permutation of coordinates' discretizations + std::vector evals; + + JorisFBP(); + double getLength(const SimTK::State&) const override; + double getLengtheningSpeed(const SimTK::State&) const override; + double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) 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; + void extendFinalizeFromProperties() override; + void extendFinalizeConnections(OpenSim::Component&) override; + }; + + 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} + { + } + }; + + // 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( + JorisFBP& impl, + const OpenSim::Model& model, + const OpenSim::PointBasedPath& pbp, + const FittingParams& params) + { + // copy model, so we can independently equilibrate + realize + modify the + // copy without having to touch the source model + std::unique_ptr modelClone{model.clone()}; + SimTK::State& initialState = modelClone->initSystem(); + modelClone->equilibrateMuscles(initialState); + modelClone->realizeVelocity(initialState); + + // set `coords` + impl.coords = coordsThatAffectPBP(*modelClone, pbp, initialState, params.numProbingDiscretizations, params.minProbingMomentArmChange); + if (static_cast(impl.coords.size()) > params.maxCoordsThatCanAffectPath || impl.coords.size() == 0) { + impl.coords.clear(); + return false; + } + + // set `coordAbsPaths` + impl.coordAbsPaths.clear(); + impl.coordAbsPaths.reserve(impl.coords.size()); + for (const OpenSim::Coordinate* c : impl.coords) { + impl.coordAbsPaths.push_back(c->getAbsolutePathString()); + } + + // set `discretizations` + impl.discretizations.clear(); + impl.discretizations.reserve(impl.coords.size()); + for (const OpenSim::Coordinate* c : impl.coords) { + impl.discretizations.push_back(discretizationForCoord(*c, params.numDiscretizationStepsPerDimension)); + } + + // set `evals` + SimTK_ASSERT_ALWAYS(impl.coords.size() == impl.discretizations.size(), "these should be equal by now"); + impl.evals = computeEvaluationsFromPBP(pbp, initialState, impl.coords.data(), impl.discretizations.data(), impl.coords.size()); + + return true; + } + + // init underlying implementation data from a `FunctionBasedPath`s (precomputed) properties + // + // the properties being set in the FBP usually implies that the FBP has already been built + // from a PBP at some previous point in time + 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)); + } + } + + // 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]); + } + } + + // returns interpolated path length for a given permutation of coordinate + // values + // + // this is the "heart" of the FPB algorithm. It's loosely based on the algorithm + // described here: + // + // "Two hierarchies of spline interpolations. Practical algorithms for multivariate higher order splines" + // https://arxiv.org/abs/0905.3564 + // + // `inputVals` points to a sequence of `nCoords` values that were probably + // retrieved via `Coordinate::getValue(SimTK::State const&)`. The reason + // that `inputVals` is provided externally (rather than have this implementation + // handle calling `getValue`) is because derivative calculations need to fiddle + // the input values slightly + double Impl_GetPathLength(JorisFBP const& impl, + double const* inputVals, + int nCoords) + { + SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); + SimTK_ASSERT_ALWAYS(nCoords == static_cast(impl.coords.size()), "You must call this function with the correct number of (precomputed) coordinate values"); + + // compute: + // + // - the index of the first discretization step *before* the input value + // + // - the polynomial of the curve at that step, given its fractional distance + // toward the next step + using Polynomial = std::array; + std::array closestDiscretizationSteps; + std::array betas; + for (int coord = 0; coord < nCoords; ++coord) { + double inputVal = inputVals[coord]; + Discretization const& disc = impl.discretizations[coord]; + double step = (disc.end - disc.begin) / (disc.nsteps - 1); + + // compute index of first discretization step *before* the input value and + // the fraction that the input value is towards the *next* discretization step + int idx; + double frac; + if (inputVal < disc.begin+step) { + idx = 1; + frac = 0.0; + } else if (inputVal > disc.end-2*step) { + idx = disc.nsteps-3; + frac = 0.0; + } else { + // solve for `n`: inputVal = begin + n*step + double n = (inputVal - disc.begin) / step; + double wholePart; + double fractionalPart = std::modf(n, &wholePart); + + idx = static_cast(wholePart); + frac = fractionalPart; + } + + // compute polynomial based on fraction the point is toward the next point + double frac2 = frac*frac; + double frac3 = frac2*frac; + double frac4 = frac3*frac; + double fracMinusOne = frac - 1; + double fracMinusOne3 = fracMinusOne*fracMinusOne*fracMinusOne; + + Polynomial p; + p[0] = 0.5 * fracMinusOne3*frac*(2*frac + 1); + p[1] = -0.5 * (frac - 1)*(6*frac4 - 9*frac3 + 2*frac + 2); + p[2] = 0.5 * frac*(6*frac4 - 15*frac3 + 9*frac2 + frac + 1); + p[3] = -0.5 * (frac - 1)*frac3*(2*frac - 3); + + closestDiscretizationSteps[coord] = idx; + betas[coord] = p; + } + + // for each coord, permute through 4 locations *around* the input's location: + // + // - A one step before B + // - B the first discretization step before the input value + // - C one step after B + // - D one step after C + // + // where: + // + // - betas are coefficients that affect each location. beta[0] affects A, + // betas[1] affects B, betas[2] affects C, and betas[3] affects D + + // represent permuting through each location around each coordinate as a string + // of integer offsets that can be -1, 0, 1, or 2 + // + // the algorithm increments this array as it goes through each permutation + std::array dimIdxOffsets; + for (int coord = 0; coord < nCoords; ++coord) { + dimIdxOffsets[coord] = -1; + } + + // permute through all locations around the input value + // + // e.g. the location permutations for 3 coords iterate like this for each + // crank of the loop + // + // [-1, -1, -1] + // [-1, -1, 0] + // [-1, -1, 1] + // [-1, -1, 2] + // [-1, 0, -1] + // ...(4^3 steps total)... + // [ 2, 2, 1] + // [ 2, 2, 2] + // [ 3, 0, 0] (the termination condition) + + double z = 0.0; + int cnt = 0; + while (dimIdxOffsets[0] < 3) { + + // compute `beta` (weighted coefficient per coord) for this particular + // permutation's coordinate locations (e.g. -1, 0, 0, 2) and figure out + // what the closest input value was at the weighted location. Add the + // result the the output + + double beta = 1.0; + int evalStride = 1; + int evalIdx = 0; + + // go backwards, from least-significant coordinate (highest idx) + // + // this is so that we can compute the stride as the algorithm runs + for (int coord = nCoords-1; coord >= 0; --coord) { + int offset = dimIdxOffsets[coord]; // -1, 0, 1, or 2 + int closestStep = closestDiscretizationSteps[coord]; + int step = closestStep + offset; + + beta *= betas[coord][offset+1]; + evalIdx += evalStride * step; + evalStride *= impl.discretizations[coord].nsteps; + } + + // equivalent to z += b*v, but handles rounding errors when the rhs + // is very small + z = std::fma(beta, impl.evals.at(evalIdx), z); + + // increment the offsets + // + // this is effectively the step that permutes [-1, -1, 2] --> [-1, 0, -1] + { + int pos = nCoords-1; + ++dimIdxOffsets[pos]; // perform least-significant increment (may overflow) + while (pos > 0 && dimIdxOffsets[pos] > 2) { // handle overflows + carry propagation + dimIdxOffsets[pos] = -1; // overflow + ++dimIdxOffsets[pos-1]; // carry propagation + --pos; + } + } + + ++cnt; + } + + // sanity check: is `z` accumulated from the expected number of iterations? + { + int expectedIterations = 1 << (2*nCoords); + if (cnt != expectedIterations) { + std::stringstream msg; + msg << "invalid number of permutations explored: expected = " << expectedIterations << ", got = " << cnt; + OPENSIM_THROW(OpenSim::Exception, std::move(msg).str()); + } + } + + return z; + } + + // get the length of the path in the current state + double Impl_GetPathLength(JorisFBP const& impl, SimTK::State const& s) + { + int nCoords = static_cast(impl.coords.size()); + + // get the input value of each coordinate in the current state + std::array inputVals{}; + for (int coord = 0; coord < nCoords; ++coord) { + inputVals[coord] = impl.coords[coord]->getValue(s); + } + + return Impl_GetPathLength(impl, inputVals.data(), nCoords); + } + + // get the *derivative* of the path length with respect to the given Coordinate index + // (in impl.coords) + //static double Impl_GetPathLengthDerivative(OpenSim::FunctionBasedPath::Impl const& impl, + // SimTK::State const& s, + // int coordIdx) { + + // SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); + // SimTK_ASSERT_ALWAYS(coordIdx != -1, "coord index must be valid"); + // SimTK_ASSERT_ALWAYS(coordIdx < static_cast(impl.coords.size()), "coord index must be valid"); + + // int nCoords = static_cast(impl.coords.size()); + + // // get the input value of each coordinate in the current state + // std::array inputVals{}; + // for (int coord = 0; coord < nCoords; ++coord) { + // inputVals[coord] = impl.coords[coord]->getValue(s); + // } + + // // compute value at current point + // double v1 = Impl_GetPathLength(impl, inputVals.data(), nCoords); + + // static constexpr double h = 0.00001; + + // // alter the input value for the to-be-derived coordinate *slightly* and recompute + // inputVals[coordIdx] += h; + // double v2 = Impl_GetPathLength(impl, inputVals.data(), nCoords); + + // // the derivative is how much the output changed when the input was altered + // // slightly (this is a poor-man's discrete derivative method) + // return (v2 - v1) / h; + //} + double Impl_GetPathLengthDerivative(JorisFBP const& impl, + SimTK::State const& s, + int coordIdx) + { + int nCoords = static_cast(impl.coords.size()); + + SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); + SimTK_ASSERT_ALWAYS(nCoords == static_cast(impl.coords.size()), "You must call this function with the correct number of (precomputed) coordinate values"); + + // get the input value of each coordinate in the current state + std::array inputVals{}; + for (int coord = 0; coord < nCoords; ++coord) { + inputVals[coord] = impl.coords[coord]->getValue(s); + } + + // compute: + // + // - the index of the first discretization step *before* the input value + // + // - the polynomial of the curve at that step, given its fractional distance + // toward the next step + using Polynomial = std::array; + std::array closestDiscretizationSteps; + std::array betas; + for (int coord = 0; coord < nCoords; ++coord) { + double inputVal = inputVals[coord]; + Discretization const& disc = impl.discretizations[coord]; + double step = (disc.end - disc.begin) / (disc.nsteps - 1); + + // compute index of first discretization step *before* the input value and + // the fraction that the input value is towards the *next* discretization step + int idx; + double frac; + if (inputVal < disc.begin+step) { + idx = 1; + frac = 0.0; + } else if (inputVal > disc.end-2*step) { + idx = disc.nsteps-3; + frac = 0.0; + } else { + // solve for `n`: inputVal = begin + n*step + double n = (inputVal - disc.begin) / step; + double wholePart; + double fractionalPart = std::modf(n, &wholePart); + + idx = static_cast(wholePart); + frac = fractionalPart; + } + + // compute polynomial based on fraction the point is toward the next point + double frac2 = frac*frac; + double frac3 = frac2*frac; + double frac4 = frac3*frac; + double fracMinusOne = frac - 1; + double fracMinusOne3 = fracMinusOne*fracMinusOne*fracMinusOne; + + Polynomial p; + if (coord == coordIdx){ + // derivative + p[0] = 5*frac4 - 10*frac3 + 4.5*frac2 + frac - 0.5; + p[1] = -15*frac4 + 30*frac3 - 13.5*frac2 - 2*frac; + p[2] = 15*frac4 - 30*frac3 + 13.5*frac2 + frac + 0.5; + p[3] = frac2*(-5*frac2 + 10*frac - 4.5); + } else { + // 'normal' spline function + p[0] = 0.5 * fracMinusOne3*frac*(2*frac + 1); + p[1] = -0.5 * (frac - 1)*(6*frac4 - 9*frac3 + 2*frac + 2); + p[2] = 0.5 * frac*(6*frac4 - 15*frac3 + 9*frac2 + frac + 1); + p[3] = -0.5 * (frac - 1)*frac3*(2*frac - 3); + } + + closestDiscretizationSteps[coord] = idx; + betas[coord] = p; + } + + std::array dimIdxOffsets; + for (int coord = 0; coord < nCoords; ++coord) { + dimIdxOffsets[coord] = -1; + } + + double z = 0.0; + int cnt = 0; + while (dimIdxOffsets[0] < 3) { + + double beta = 1.0; + int evalStride = 1; + int evalIdx = 0; + + for (int coord = nCoords-1; coord >= 0; --coord) { + int offset = dimIdxOffsets[coord]; // -1, 0, 1, or 2 + int closestStep = closestDiscretizationSteps[coord]; + int step = closestStep + offset; + + beta *= betas[coord][offset+1]; + evalIdx += evalStride * step; + evalStride *= impl.discretizations[coord].nsteps; + } + + double gridSize = (impl.discretizations[coordIdx].end-impl.discretizations[coordIdx].begin)/impl.discretizations[coordIdx].nsteps; + z = std::fma(beta, impl.evals.at(evalIdx)/gridSize, z); + { + int pos = nCoords-1; + ++dimIdxOffsets[pos]; // perform least-significant increment (may overflow) + while (pos > 0 && dimIdxOffsets[pos] > 2) { // handle overflows + carry propagation + dimIdxOffsets[pos] = -1; // overflow + ++dimIdxOffsets[pos-1]; // carry propagation + --pos; + } + } + + ++cnt; + } + { + int expectedIterations = 1 << (2*nCoords); + if (cnt != expectedIterations) { + std::stringstream msg; + msg << "invalid number of permutations explored: expected = " << expectedIterations << ", got = " << cnt; + OPENSIM_THROW(OpenSim::Exception, std::move(msg).str()); + } + } + + return z; + } + + // get the *derivative* of the path length with respect to the given Coordinate + double Impl_GetPathLengthDerivative(JorisFBP const& impl, + SimTK::State const& s, + OpenSim::Coordinate const& c) + { + // figure out the index of the coordinate being referred to + int coordIdx = -1; + for (int i = 0; i < static_cast(impl.coords.size()); ++i) { + if (impl.coords[i] == &c) { + coordIdx = i; + break; + } + } + + // ensure the coordinate was actually found, or this alg will break + if (coordIdx == -1) { + std::stringstream msg; + msg << "could not find coordiante '" << c.getName() << "' in the set of coordinates the FunctionBasedPath handles. Coordinates handled by this path are: "; + char const* delim = ""; + for (OpenSim::Coordinate const* c : impl.coords) { + msg << delim << c->getName(); + delim = ", "; + } + OPENSIM_THROW(OpenSim::Exception, std::move(msg).str()); + } + + // use the "raw" (non-lookup) version of this function with the index + return Impl_GetPathLengthDerivative(impl, s, coordIdx); + } + + // get the lengthening speed of the path in the current state + double Impl_GetLengtheningSpeed(const JorisFBP& impl, + const SimTK::State& state) + { + double lengtheningSpeed = 0.0; + for (int coordIdx = 0; coordIdx < static_cast(impl.coords.size()); ++coordIdx) { + double deriv = Impl_GetPathLengthDerivative(impl, state, coordIdx); + double coordSpeedVal = impl.coords[coordIdx]->getSpeedValue(state); + + lengtheningSpeed = std::fma(deriv, coordSpeedVal, lengtheningSpeed); + } + return lengtheningSpeed; + } + + 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; + } + + JorisFBP::JorisFBP() + { + constructProperty_FunctionBasedPathDiscretizationSet(FunctionBasedPathDiscretizationSet{}); + constructProperty_Evaluations(); + } + + double JorisFBP::getLength(const SimTK::State& s) const + { + return Impl_GetPathLength(*this, s); + } + + double JorisFBP::getLengtheningSpeed(const SimTK::State& s) const + { + return Impl_GetLengtheningSpeed(*this, s); + } + + double JorisFBP::computeMomentArm(const SimTK::State& s, const OpenSim::Coordinate& aCoord) const + { + return Impl_GetPathLengthDerivative(*this, s, aCoord); + } + + void JorisFBP::getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const + { + OPENSIM_THROW(Exception, "Tried to call `getPointForceDirections` on a `JorisFBP`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it isn't path-based). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); + } + + void JorisFBP::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const + { + const SimTK::SimbodyMatterSubsystem& matter = getModel().getMatterSubsystem(); + + for (const OpenSim::Coordinate* coord : coords) { + double ma = computeMomentArm(state, *coord); + double torqueOverCoord = -tension*ma; + + matter.addInMobilityForce(state, + SimTK::MobilizedBodyIndex(coord->getBodyIndex()), + SimTK::MobilizerUIndex(coord->getMobilizerQIndex()), + torqueOverCoord, + mobilityForces); + } + } + + void JorisFBP::extendFinalizeFromProperties() + { + Impl_InitFromFBPProperties(*this); + } + + void JorisFBP::extendFinalizeConnections(OpenSim::Component& root) + { + // populate pointer-based coordinate lookups + // + // the reason this isn't done in `extendFinalizeFromProperties` is because the + // not-yet-property-finalized Model hasn't necessarily "connected" to the + // coordinates that the coordinate files refer to, so the implementation + // can't lookup the `OpenSim::Coordinate*` pointers during that phase + + // Allow (model) component to include its own subcomponents + // before calling the base method which automatically invokes + // connect all the subcomponents. + { + Model* model = dynamic_cast(&root); + if (model) { + connectToModel(*model); + } + } + + Impl_SetCoordinatePointersFromCoordinatePaths(*this, root); + } + + OSIM_TEST(JorisFBP, CanBeDefaultConstructed) + { + JorisFBP fbp; + } + + // TODO: port `Applications/FunctionBasedPathConversion/test/testFunctionBasedPathConversion.cpp` + // see: `https://github.com/joris997/opensim-core/tree/interpolation/Applications/FunctionBasedPathConversion` +} + int main() { return RunAllTests(); From bf1fde401d0143f1ed05379734ee497d387c56a7 Mon Sep 17 00:00:00 2001 From: Adam Kewley Date: Thu, 2 Dec 2021 16:34:22 +0100 Subject: [PATCH 08/12] Fixed missing OSIMSIMULATION_API macro in FunctionBasedPath decl --- OpenSim/Simulation/Model/FunctionBasedPath.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h index 534e3e5e8c..6d54dbd842 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.h +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -32,7 +32,7 @@ class PointForceDirection; * that automatically handles forwarding calls to the `PathFunction`, state * caching, etc. */ -class PathFunction : public OpenSim::ModelComponent { +class OSIMSIMULATION_API PathFunction : public OpenSim::ModelComponent { OpenSim_DECLARE_ABSTRACT_OBJECT(PathFunction, OpenSim::Component) public: @@ -52,7 +52,7 @@ class PathFunction : public OpenSim::ModelComponent { * actual path, and also handles any `GeometryPath`-specific concerns, such as * handling coloring and caching results. */ -class FunctionBasedPath final : public GeometryPath { +class OSIMSIMULATION_API FunctionBasedPath final : public GeometryPath { OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, GeometryPath); OpenSim_DECLARE_PROPERTY(PathFunction, PathFunction, "The underlying function that is used at simulation-time to evaluate the length, lengthening speed, and moment arm of the path."); From 01c3dbf3baf2c0e4eb394dffc113c4163fa212fa Mon Sep 17 00:00:00 2001 From: Adam Kewley Date: Fri, 3 Dec 2021 09:41:14 +0100 Subject: [PATCH 09/12] Made getPointForceDirections a deprecated method with a stub implementation --- .../Simulation/Model/FunctionBasedPath.cpp | 12 +-- OpenSim/Simulation/Model/FunctionBasedPath.h | 5 - OpenSim/Simulation/Model/GeometryPath.cpp | 100 +++++++++++------- OpenSim/Simulation/Model/GeometryPath.h | 24 ++--- .../Simulation/Test/testFunctionBasedPath.cpp | 83 +-------------- 5 files changed, 80 insertions(+), 144 deletions(-) diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index 46d40bae80..6d03fae957 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -30,12 +30,7 @@ namespace { OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); } - void getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const override - { - OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); - } - - void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const override + void addInEquivalentForces(const SimTK::State&, double, SimTK::Vector_&, SimTK::Vector&) const override { OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); } @@ -94,11 +89,6 @@ double OpenSim::FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) co return v; } -void OpenSim::FunctionBasedPath::getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const -{ - getProperty_PathFunction().getValue().getPointForceDirections(s, rPFDs); -} - void OpenSim::FunctionBasedPath::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const { getProperty_PathFunction().getValue().addInEquivalentForces(state, tension, bodyForces, mobilityForces); diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h index 6d54dbd842..fdccadc5a8 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.h +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -41,7 +41,6 @@ class OSIMSIMULATION_API PathFunction : public OpenSim::ModelComponent { virtual double getLength(const SimTK::State&) const = 0; virtual double getLengtheningSpeed(const SimTK::State&) const = 0; virtual double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const = 0; - virtual void getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const = 0; virtual void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const = 0; }; @@ -77,10 +76,6 @@ class OSIMSIMULATION_API FunctionBasedPath final : public GeometryPath { double getLength(const SimTK::State& s) const override; double getLengtheningSpeed(const SimTK::State& s) const override; - void getPointForceDirections( - const SimTK::State& s, - OpenSim::Array* rPFDs) const override; - void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 2acd445af3..5d4dbbaef5 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -159,24 +159,30 @@ static bool emitDeprecationWarning(char const* funcName) return true; } +#define SHOW_FUNC_DEPRECATION_WARNING() \ + static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); \ + (void)g_ShownDeprecationWarning; + const OpenSim::PathPointSet& OpenSim::GeometryPath::getPathPointSet() const { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return _impl->getOrUpdPPSCached(); } OpenSim::PathPointSet& OpenSim::GeometryPath::updPathPointSet() { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return _impl->getOrUpdPPSCached(); } OpenSim::AbstractPathPoint* OpenSim::GeometryPath::addPathPoint( - const SimTK::State& s, - int index, - const PhysicalFrame& frame) + const SimTK::State&, + int, + const PhysicalFrame&) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); // 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 @@ -185,11 +191,11 @@ OpenSim::AbstractPathPoint* OpenSim::GeometryPath::addPathPoint( } OpenSim::AbstractPathPoint* OpenSim::GeometryPath::appendNewPathPoint( - const std::string& proposedName, - const PhysicalFrame& frame, - const SimTK::Vec3& locationOnFrame) + const std::string&, + const PhysicalFrame&, + const SimTK::Vec3&) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); // 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 @@ -197,83 +203,105 @@ OpenSim::AbstractPathPoint* OpenSim::GeometryPath::appendNewPathPoint( return _impl->getOrUpdFixupPathPointCached(); } -bool OpenSim::GeometryPath::canDeletePathPoint(int index) +bool OpenSim::GeometryPath::canDeletePathPoint(int) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return false; } -bool OpenSim::GeometryPath::deletePathPoint(const SimTK::State& s, int index) +bool OpenSim::GeometryPath::deletePathPoint(const SimTK::State&, int) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return false; } bool OpenSim::GeometryPath::replacePathPoint( - const SimTK::State& s, - AbstractPathPoint* oldPathPoint, - AbstractPathPoint* newPathPoint) + const SimTK::State&, + AbstractPathPoint*, + AbstractPathPoint*) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return false; } -const OpenSim::Array& OpenSim::GeometryPath::getCurrentPath(const SimTK::State& s) const +const OpenSim::Array& OpenSim::GeometryPath::getCurrentPath(const SimTK::State&) const { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return _impl->getOrUpdCurrentPathArray(); } const OpenSim::PathWrapSet& OpenSim::GeometryPath::getWrapSet() const { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return _impl->getOrUpdPathWrapSet(); } OpenSim::PathWrapSet& OpenSim::GeometryPath::updWrapSet() { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return _impl->getOrUpdPathWrapSet(); } -void OpenSim::GeometryPath::addPathWrap(WrapObject &aWrapObject) +void OpenSim::GeometryPath::addPathWrap(WrapObject&) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return; } -void OpenSim::GeometryPath::moveUpPathWrap(const SimTK::State& s, int index) +void OpenSim::GeometryPath::moveUpPathWrap(const SimTK::State&, int) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return; } -void OpenSim::GeometryPath::moveDownPathWrap(const SimTK::State& s, int index) +void OpenSim::GeometryPath::moveDownPathWrap(const SimTK::State&, int) { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + + return; +} + +void OpenSim::GeometryPath::deletePathWrap(const SimTK::State&, int) +{ + SHOW_FUNC_DEPRECATION_WARNING(); + return; } -void OpenSim::GeometryPath::deletePathWrap(const SimTK::State& s, int index) +void OpenSim::GeometryPath::setLength(const SimTK::State&, double) const { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return; } -void OpenSim::GeometryPath::setLength(const SimTK::State& s, double length) const +void OpenSim::GeometryPath::setLengtheningSpeed(const SimTK::State&, double) const { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return; } -void OpenSim::GeometryPath::setLengtheningSpeed(const SimTK::State& s, double speed) const +void OpenSim::GeometryPath::getPointForceDirections( + const SimTK::State&, + OpenSim::Array*) const { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return; } -void OpenSim::GeometryPath::updateGeometry(const SimTK::State& s) const +void OpenSim::GeometryPath::updateGeometry(const SimTK::State&) const { - static const bool g_ShownDeprecationWarning = emitDeprecationWarning(__func__); + SHOW_FUNC_DEPRECATION_WARNING(); + return; } diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index c994a0bdf1..d80b930a4e 100644 --- a/OpenSim/Simulation/Model/GeometryPath.h +++ b/OpenSim/Simulation/Model/GeometryPath.h @@ -153,18 +153,6 @@ class OSIMSIMULATION_API GeometryPath : public ModelComponent { */ virtual double getLengtheningSpeed(const SimTK::State& s) const = 0; - /** - * 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 = 0; - /** * Add in the equivalent body and generalized forces to be applied to the * multibody system resulting from a tension along the GeometryPath. @@ -355,6 +343,18 @@ class OSIMSIMULATION_API GeometryPath : public ModelComponent { 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. * diff --git a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp index 9e613e831d..45ba3b2fdf 100644 --- a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -15,7 +15,7 @@ namespace { std::ostream& operator<<(std::ostream& o, Test const& t) { - return o << t.suiteName << ':' << t.name; + return o << t.suiteName << '/' << t.name; } static std::vector& GetTestList() @@ -93,9 +93,7 @@ namespace { int numTimesComputeMomentArmCalled = 0; double computeMomentArmValue = GenerateDouble(); - int numTimesGetPointForceDirectionsCalled = 0; int numTimesAddInEquivalentForcesCalled = 0; - int numTimesExtendConnectToModelCalled = 0; int numTimesExtendInitStateFromPropertiesCalled = 0; int numTimesExtendAddToSystemCalled = 0; @@ -126,12 +124,7 @@ namespace { return data->computeMomentArmValue; } - void getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const override - { - ++data->numTimesGetPointForceDirectionsCalled; - } - - void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const override + void addInEquivalentForces(const SimTK::State&, double, SimTK::Vector_&, SimTK::Vector&) const override { ++data->numTimesAddInEquivalentForcesCalled; } @@ -372,68 +365,6 @@ OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedIsCached) SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 1); } -OSIM_TEST(FunctionBasedPath, CanCallGetPointForceDirectionWithoutThrowing) -{ - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); - - SimTK::State& s = model.initSystem(); - - OpenSim::Array pfds; - fbp->getPointForceDirections(s, &pfds); // shouldn't throw -} - -OSIM_TEST(FunctionBasedPath, GetPointForceDirectionUsesPathFunctionImpl) -{ - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); - - SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); - - SimTK::State& s = model.initSystem(); - - SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); - - { - OpenSim::Array pfds; - fbp->getPointForceDirections(s, &pfds); - } - - SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 1); -} - -OSIM_TEST(FunctionBasedPath, GetPointForceDirectionsIsNotCached) -{ - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); - - SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); - - SimTK::State& s = model.initSystem(); - - SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 0); - - { - OpenSim::Array pfds; - fbp->getPointForceDirections(s, &pfds); - } - - SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 1); - - { - OpenSim::Array pfds; - fbp->getPointForceDirections(s, &pfds); // not cached - } - - SimTK_TEST(pathFn.data->numTimesGetPointForceDirectionsCalled == 2); -} - OSIM_TEST(FunctionBasedPath, CanCallAddInEquivalentForcesWithoutThrowing) { OpenSim::Model model; @@ -501,7 +432,6 @@ OSIM_TEST(FunctionBasedPath, AddInEquivalentForcesIsNotCached) SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 2); } - OSIM_TEST(FunctionBasedPath, CanCallComputeMomentArmWithoutThrowing) { OpenSim::Model model; @@ -599,7 +529,6 @@ OSIM_TEST(FunctionBasedPath, PathFunctionExtendFinalizePropertiesCalledWhenFinal } - // HACK: test Joris's implementation here // // this is because I cba'd splitting OSIM_TEST into another compilation unit, and because @@ -835,7 +764,6 @@ namespace joris { double getLength(const SimTK::State&) const override; double getLengtheningSpeed(const SimTK::State&) const override; double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) 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; void extendFinalizeFromProperties() override; void extendFinalizeConnections(OpenSim::Component&) override; @@ -1448,12 +1376,7 @@ namespace joris { return Impl_GetPathLengthDerivative(*this, s, aCoord); } - void JorisFBP::getPointForceDirections(const SimTK::State& s, OpenSim::Array* rPFDs) const - { - OPENSIM_THROW(Exception, "Tried to call `getPointForceDirections` on a `JorisFBP`. You cannot call this method on a `GeometryPath` that is a `FunctionBasedPath` (it isn't path-based). Either remove this function call or replace the `FunctionBasedPath` with a `PointBasedPath` in the model"); - } - - void JorisFBP::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const + void JorisFBP::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_&, SimTK::Vector& mobilityForces) const { const SimTK::SimbodyMatterSubsystem& matter = getModel().getMatterSubsystem(); From aacd2d53f98207d031dc07f27ea5ee7d9228e2f7 Mon Sep 17 00:00:00 2001 From: Adam Kewley Date: Fri, 3 Dec 2021 16:50:41 +0100 Subject: [PATCH 10/12] Fix for VS2013, implement basic OpenSim::Function plumbing + tests --- .../Simulation/Model/FunctionBasedPath.cpp | 250 ++- OpenSim/Simulation/Model/FunctionBasedPath.h | 44 +- OpenSim/Simulation/Model/GeometryPath.cpp | 4 - OpenSim/Simulation/Model/GeometryPath.h | 2 - .../Simulation/Test/testFunctionBasedPath.cpp | 1366 +++++++++-------- 5 files changed, 982 insertions(+), 684 deletions(-) diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index 6d03fae957..fc37dec43d 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -4,59 +4,140 @@ #include #include +#include + namespace { - // used as a stub PathFunction implementation that throws if the user tries - // to actually use it + 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 // - // handy for handling the case where we need *some* PathFunction to stand-in - // before (e.g.) a "proper" path function is assigned to a property or - // something. - static char const g_ThrowingPathErrMsg[] = "Cannot call any member on a ThrowingPathFunction. This exception has *probably* been throwng because a PathFunction has not (yet) been assigned for the parent component. You *probably* need to ensure that a FunctionBasedPath actually has PathFunction assigned to it"; - class ThrowingPathFunction final : public OpenSim::PathFunction { - OpenSim_DECLARE_CONCRETE_OBJECT(ThrowingPathFunction, OpenSim::PathFunction) - - double getLength(const SimTK::State&) const override + // 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 { - OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + throw InvalidFunctionError(m_ParentPath, __func__); } - double getLengtheningSpeed(const SimTK::State&) const override + double calcDerivative(const std::vector&, const SimTK::Vector&) const override { - OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + throw InvalidFunctionError(m_ParentPath, __func__); } - double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const override + int getArgumentSize() const override { - OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + throw InvalidFunctionError(m_ParentPath, __func__); } - void addInEquivalentForces(const SimTK::State&, double, SimTK::Vector_&, SimTK::Vector&) const override + int getMaxDerivativeOrder() const override { - OPENSIM_THROW_FRMOBJ(OpenSim::Exception, g_ThrowingPathErrMsg); + 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(ThrowingPathFunction{}); + constructProperty_PathFunction(ThrowingOpenSimFunction{this->getAbsolutePathString()}); + constructProperty_Coordinates(); } OpenSim::FunctionBasedPath::FunctionBasedPath(const FunctionBasedPath&) = default; -OpenSim::FunctionBasedPath::FunctionBasedPath(FunctionBasedPath&&) noexcept = default; - -OpenSim::FunctionBasedPath::FunctionBasedPath(PathFunction const& pathFn) : GeometryPath{} +OpenSim::FunctionBasedPath::FunctionBasedPath(const OpenSim::Function& func, std::vector coordAbsPaths) { - constructProperty_PathFunction(pathFn); + 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; -OpenSim::FunctionBasedPath& OpenSim::FunctionBasedPath::operator=(FunctionBasedPath&&) noexcept = default; - SimTK::Vec3 OpenSim::FunctionBasedPath::getColor(const SimTK::State& s) const { return getCacheVariableValue(s, _colorCV); @@ -73,7 +154,8 @@ double OpenSim::FunctionBasedPath::getLength(const SimTK::State& s) const return getCacheVariableValue(s, _lengthCV); } - double v = getProperty_PathFunction().getValue().getLength(s); + const SimTK::Vector& args = calcFunctionArguments(s); + double v = getProperty_PathFunction().getValue().calcValue(args); setCacheVariableValue(s, _lengthCV, v); return v; } @@ -84,19 +166,71 @@ double OpenSim::FunctionBasedPath::getLengtheningSpeed(const SimTK::State& s) co return getCacheVariableValue(s, _speedCV); } - double v = getProperty_PathFunction().getValue().getLengtheningSpeed(s); - setCacheVariableValue(s, _speedCV, v); - return v; + // 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_& bodyForces, SimTK::Vector& mobilityForces) const +void OpenSim::FunctionBasedPath::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_&, SimTK::Vector& mobilityForces) const { - getProperty_PathFunction().getValue().addInEquivalentForces(state, tension, bodyForces, mobilityForces); + 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& s, const Coordinate& aCoord) const +double OpenSim::FunctionBasedPath::computeMomentArm(const SimTK::State& st, const Coordinate& coord) const { - return getProperty_PathFunction().getValue().computeMomentArm(s, aCoord); + // 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 @@ -119,3 +253,55 @@ void OpenSim::FunctionBasedPath::extendInitStateFromProperties(SimTK::State& s) 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; +} diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.h b/OpenSim/Simulation/Model/FunctionBasedPath.h index fdccadc5a8..954efc0629 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.h +++ b/OpenSim/Simulation/Model/FunctionBasedPath.h @@ -2,6 +2,7 @@ #define OPENSIM_FUNCTIONBASED_PATH_H_ #include +#include #include #include #include @@ -23,27 +24,6 @@ namespace OpenSim { class Coordinate; class PointForceDirection; -/** - * An interface for an object that can compute the length, lengthening speed, - * and moment arm (w.r.t. a particular `OpenSim::Coordinate`) of a path at - * runtime. - * - * See `FunctionBasedPath` for a standard implementation of a `GeometryPath` - * that automatically handles forwarding calls to the `PathFunction`, state - * caching, etc. - */ -class OSIMSIMULATION_API PathFunction : public OpenSim::ModelComponent { - OpenSim_DECLARE_ABSTRACT_OBJECT(PathFunction, OpenSim::Component) - -public: - virtual ~PathFunction() noexcept = default; - - virtual double getLength(const SimTK::State&) const = 0; - virtual double getLengtheningSpeed(const SimTK::State&) const = 0; - virtual double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const = 0; - virtual void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const = 0; -}; - /** * An `OpenSim::GeometryPath` that uses `PathFunction`s to compute its state. * @@ -54,21 +34,30 @@ class OSIMSIMULATION_API PathFunction : public OpenSim::ModelComponent { class OSIMSIMULATION_API FunctionBasedPath final : public GeometryPath { OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPath, GeometryPath); - OpenSim_DECLARE_PROPERTY(PathFunction, PathFunction, "The underlying function that is used at simulation-time to evaluate the length, lengthening speed, and moment arm of the path."); + 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&); - FunctionBasedPath(FunctionBasedPath&&) noexcept; - explicit FunctionBasedPath(PathFunction const&); + + /** + * 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&); - FunctionBasedPath& operator=(FunctionBasedPath&&) noexcept; SimTK::Vec3 getColor(const SimTK::State& s) const override; void setColor(const SimTK::State& s, const SimTK::Vec3& color) const override; @@ -84,8 +73,13 @@ class OSIMSIMULATION_API FunctionBasedPath final : public GeometryPath { 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; }; } diff --git a/OpenSim/Simulation/Model/GeometryPath.cpp b/OpenSim/Simulation/Model/GeometryPath.cpp index 5d4dbbaef5..f4066eebcd 100644 --- a/OpenSim/Simulation/Model/GeometryPath.cpp +++ b/OpenSim/Simulation/Model/GeometryPath.cpp @@ -105,14 +105,10 @@ OpenSim::GeometryPath::GeometryPath() : ModelComponent{}, _impl{new Impl{}} OpenSim::GeometryPath::GeometryPath(OpenSim::GeometryPath const&) = default; -OpenSim::GeometryPath::GeometryPath(OpenSim::GeometryPath&&) noexcept = default; - OpenSim::GeometryPath::~GeometryPath() noexcept = default; OpenSim::GeometryPath& OpenSim::GeometryPath::operator=(const GeometryPath&) = default; -OpenSim::GeometryPath& OpenSim::GeometryPath::operator=(GeometryPath&&) noexcept = default; - // DEFAULTED METHODS diff --git a/OpenSim/Simulation/Model/GeometryPath.h b/OpenSim/Simulation/Model/GeometryPath.h index d80b930a4e..fdf746b5aa 100644 --- a/OpenSim/Simulation/Model/GeometryPath.h +++ b/OpenSim/Simulation/Model/GeometryPath.h @@ -96,11 +96,9 @@ class OSIMSIMULATION_API GeometryPath : public ModelComponent { public: GeometryPath(); GeometryPath(const GeometryPath&); - GeometryPath(GeometryPath&&) noexcept; ~GeometryPath() noexcept; GeometryPath& operator=(const GeometryPath&); - GeometryPath& operator=(GeometryPath&&) noexcept; // INTERFACE METHODS diff --git a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp index 45ba3b2fdf..c8e3d54333 100644 --- a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -1,5 +1,7 @@ #include +#include +#include #include #include @@ -8,8 +10,9 @@ #include #include -// this is a poor-man's GoogleTest, but I still prefer it to the madness of -// having a massive `main()` containing many `try..catch` blocks +// 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); }; @@ -83,122 +86,294 @@ namespace { return SimTK::Vec3{GenerateDouble(), GenerateDouble(), GenerateDouble()}; } - struct MockPathFunctionData { - int numTimesGetLengthCalled = 0; - double lengthValue = 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; + } +} - int numTimesLengtheningSpeedCalled = 0; - double lengtheningSpeedValue = GenerateDouble(); +// 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; + } - int numTimesComputeMomentArmCalled = 0; - double computeMomentArmValue = GenerateDouble(); + 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; + } - int numTimesAddInEquivalentForcesCalled = 0; - int numTimesExtendConnectToModelCalled = 0; - int numTimesExtendInitStateFromPropertiesCalled = 0; - int numTimesExtendAddToSystemCalled = 0; - int numTimesExtendFinalizeFromPropertiesCalled = 0; + 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; }; - // used to test that the FunctionBasedPath is forwarding things correctly - class MockPathFunction : public OpenSim::PathFunction { - OpenSim_DECLARE_CONCRETE_OBJECT(MockPathFunction, OpenSim::PathFunction); + class MockSimTKFunction final : public SimTK::Function { public: - mutable std::shared_ptr data = std::make_shared(); + mutable std::shared_ptr m_SharedData; + + MockSimTKFunction(std::shared_ptr sharedData) : + m_SharedData{sharedData} + { + } - double getLength(const SimTK::State&) const override + double calcValue(const SimTK::Vector& args) const override { - ++data->numTimesGetLengthCalled; - return data->lengthValue; + ++m_SharedData->numTimesCalcValueCalled; + m_SharedData->lastCalcValueArg = ToStdVector(args); + return m_SharedData->value; } - double getLengtheningSpeed(const SimTK::State&) const override + double calcDerivative(const SimTK::Array_& intArg, const SimTK::Vector& valArgs) const override { - ++data->numTimesLengtheningSpeedCalled; - return data->lengtheningSpeedValue; + ++m_SharedData->numTimesCalcDerivativeCalled; + m_SharedData->lastCalcDerivativeIntArg = ToStdVector(intArg); + m_SharedData->lastCalcDerivativeValArgs = ToStdVector(valArgs); + return m_SharedData->derivative; } - double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const override + int getArgumentSize() const override { - ++data->numTimesComputeMomentArmCalled; - return data->computeMomentArmValue; + ++m_SharedData->numTimesGetArgumentSizeCalled; + return m_SharedData->argumentSize; } - void addInEquivalentForces(const SimTK::State&, double, SimTK::Vector_&, SimTK::Vector&) const override + int getMaxDerivativeOrder() const override { - ++data->numTimesAddInEquivalentForcesCalled; + ++m_SharedData->numTimesGetMaxDerivativeOrderCalled; + return m_SharedData->maxDerivativeOrder; } - // these are redundantly checked (the Component/Property tests *should* - // also test these) because existing implementations do use these methods - // to hook into the model/system/state at various steps (e.g. to cache - // coordinates, or whatever they need) and it's handy to redundantly - // ensure these are hooked up to the PathFunction via the FunctionBasedPath - // correctly. + 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; + } - void extendConnectToModel(OpenSim::Model&) override + double calcDerivative(const std::vector& intArgs, const SimTK::Vector& valArgs) const override { - ++data->numTimesExtendConnectToModelCalled; + ++m_SharedData->numTimesCalcDerivativeCalled; + m_SharedData->lastCalcDerivativeIntArg = intArgs; + m_SharedData->lastCalcDerivativeValArgs = ToStdVector(valArgs); + return m_SharedData->derivative; } - void extendInitStateFromProperties(SimTK::State&) const override + int getArgumentSize() const override { - ++data->numTimesExtendInitStateFromPropertiesCalled; + ++m_SharedData->numTimesGetArgumentSizeCalled; + return m_SharedData->argumentSize; } - void extendAddToSystem(SimTK::MultibodySystem&) const override + int getMaxDerivativeOrder() const override { - ++data->numTimesExtendAddToSystemCalled; + ++m_SharedData->numTimesGetMaxDerivativeOrderCalled; + return m_SharedData->maxDerivativeOrder; } - void extendFinalizeFromProperties() override + SimTK::Function* createSimTKFunction() const override { - ++data->numTimesExtendFinalizeFromPropertiesCalled; + 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, CanBeConstructedWithAPathFunction) +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; - OpenSim::FunctionBasedPath fbp{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; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + std::vector coords; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn, coords}; + model.addComponent(fbp); - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + SimTK_TEST(pathFn.m_SharedData->numTimesCalcValueCalled == 0); SimTK::State& s = model.initSystem(); - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + SimTK_TEST(pathFn.m_SharedData->numTimesCalcValueCalled == 0); - SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); + SimTK_TEST(fbp->getLength(s) == pathFn.m_SharedData->value); - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); + SimTK_TEST(pathFn.m_SharedData->numTimesCalcValueCalled == 1); } -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)}; -} +// assignment tests OSIM_TEST(FunctionBasedPath, CanBeCopyAssignedWithoutThrowing) { @@ -214,320 +389,367 @@ OSIM_TEST(FunctionBasedPath, CanBeMoveAssignedWithoutThrowing) fbp2 = std::move(fbp1); } -OSIM_TEST(FunctionBasedPath, CanGetColorWithoutThrowing) + +// api tests + +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithoutSettingFunctionThrows) { - OpenSim::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; - model.addComponent(fbp); + // calling finalizeFromProperties implies the FBP's properties are correct + // + // it is incorrect to leave the Function property not-set - SimTK::State& s = model.initSystem(); + OpenSim::FunctionBasedPath fbp; - fbp->getColor(s); + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); } -OSIM_TEST(FunctionBasedPath, GetColorReturnsDefaultColorIfSetColorIsNotCalled) +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithNonDifferentiableFunctionThrows) { - SimTK::Vec3 randomColor = GenerateRandomVector(); + // 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::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; - fbp->setDefaultColor(randomColor); - model.addComponent(fbp); + OpenSim::FunctionBasedPath fbp; - SimTK::State& s = model.initSystem(); + MockPathFunction fn; + fn.m_SharedData->maxDerivativeOrder = 0; // non-differentiable - SimTK_TEST(fbp->getColor(s) == randomColor); + auto& prop = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + prop.setValue(fn); + + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); } -OSIM_TEST(FunctionBasedPath, SetColorSetsTheColorInTheState) +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithInvalidFunctionArityThrows) { - SimTK::Vec3 randomColor = GenerateRandomVector(); + // 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::Model model; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{}; - fbp->setDefaultColor(randomColor); - model.addComponent(fbp); + OpenSim::FunctionBasedPath fbp; - SimTK::State& s = model.initSystem(); + MockPathFunction fn; + fn.m_SharedData->argumentSize = 10; - fbp->setColor(s, randomColor); + auto& prop = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + prop.setValue(fn); - SimTK_TEST(fbp->getColor(s) == randomColor); + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); } -OSIM_TEST(FunctionBasedPath, CanCallGetLengthWithoutThrowing) +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithEmptyCoordinateStringThrows) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + // 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) - SimTK::State& s = model.initSystem(); - model.realizePosition(s); + OpenSim::FunctionBasedPath fbp; - fbp->getLength(s); // shouldn't throw -} + MockPathFunction fn; + fn.m_SharedData->argumentSize = 1; + auto& funcProp = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + funcProp.setValue(fn); -OSIM_TEST(FunctionBasedPath, GetLengthUsesPathFunctionImpl) -{ - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + auto& coordProp = dynamic_cast&>(fbp.updPropertyByName("Coordinates")); + coordProp.adoptAndAppendValue(new std::string{""}); - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + ASSERT_THROW(OpenSim::Exception, fbp.finalizeFromProperties()); +} - SimTK::State& s = model.initSystem(); - model.realizePosition(s); +OSIM_TEST(FunctionBasedPath, FinalizeFromPropertiesWithCorrectPropertiesDoesNotThrow) +{ + // this is a throwaway test that ensures the sane-case doesn't throw - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + OpenSim::FunctionBasedPath fbp; - SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); + MockPathFunction fn; + fn.m_SharedData->argumentSize = 0; + auto& funcProp = dynamic_cast&>(fbp.updPropertyByName("PathFunction")); + funcProp.setValue(fn); - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); + fbp.finalizeFromProperties(); // shouldn't throw } -OSIM_TEST(FunctionBasedPath, GetLengthIsCached) +OSIM_TEST(FunctionBasedPath, FinalizeConnectionsWithCorrectPropertiesDoesNotThrow) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + // calling `finalizeConnections` should be fine if every coordinate path + // provided to the FBP constructor is actually a real coordinate - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); - SimTK::State& s = model.initSystem(); - model.realizePosition(s); + 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 - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 0); + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); - SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); + auto paths = m.coordinateAbsPaths; + paths[2] = "not-a-real-coordinate-path"; - SimTK_TEST(fbp->getLength(s) == pathFn.data->lengthValue); // should cache + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, paths}; + m.model.addComponent(fbp); - SimTK_TEST(pathFn.data->numTimesGetLengthCalled == 1); + m.model.finalizeFromProperties(); + ASSERT_THROW(OpenSim::Exception, m.model.finalizeConnections()); } -OSIM_TEST(FunctionBasedPath, CanCallGetLengtheningSpeedWithoutThrowing) +OSIM_TEST(FunctionBasedPath, CanGetColorWithoutThrowing) { OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{MockPathFunction{}, {}}; model.addComponent(fbp); SimTK::State& s = model.initSystem(); - model.realizeAcceleration(s); - fbp->getLengtheningSpeed(s); // shouldn't throw + fbp->getColor(s); } -OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedUsesPathFunctionImpl) +OSIM_TEST(FunctionBasedPath, GetColorReturnsDefaultColorIfSetColorIsNotCalled) { + SimTK::Vec3 randomColor = GenerateRandomVector(); + OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{MockPathFunction{}, {}}; + fbp->setDefaultColor(randomColor); model.addComponent(fbp); - SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); - SimTK::State& s = model.initSystem(); - model.realizeAcceleration(s); - - SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); - - SimTK_TEST(fbp->getLengtheningSpeed(s) == pathFn.data->lengtheningSpeedValue); - SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 1); + SimTK_TEST(fbp->getColor(s) == randomColor); } -OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedIsCached) +OSIM_TEST(FunctionBasedPath, SetColorSetsTheColorInTheState) { + SimTK::Vec3 randomColor = GenerateRandomVector(); + OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{MockPathFunction{}, {}}; + fbp->setDefaultColor(randomColor); model.addComponent(fbp); - SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); - SimTK::State& s = model.initSystem(); - model.realizeAcceleration(s); - SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 0); + fbp->setColor(s, randomColor); - SimTK_TEST(fbp->getLengtheningSpeed(s) == pathFn.data->lengtheningSpeedValue); + SimTK_TEST(fbp->getColor(s) == randomColor); +} - SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 1); +OSIM_TEST(FunctionBasedPath, GetLengthUsesUnderlyingFunctionCalcValue) +{ + // the FunctionBasedPath ultimately gets its result values from the underlying + // Function object, rather than generating them itself - SimTK_TEST(fbp->getLengtheningSpeed(s) == pathFn.data->lengtheningSpeedValue); // should cache + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); + OpenSim::Coordinate const& coord = m.model.getComponent(m.coordinateAbsPaths.at(0)); - SimTK_TEST(pathFn.data->numTimesLengtheningSpeedCalled == 1); -} + MockPathFunction fn; + fn.m_SharedData->argumentSize = 1; + std::vector coordPaths = {coord.getAbsolutePathString()}; -OSIM_TEST(FunctionBasedPath, CanCallAddInEquivalentForcesWithoutThrowing) -{ - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, coordPaths}; + m.model.addComponent(fbp); - SimTK::State& s = model.initSystem(); + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 0); - SimTK::Vector_ bodyForces; - SimTK::Vector mobilityForces; + SimTK::State& s = m.model.initSystem(); - fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); + 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, AddInEquivalentForcesUsesPathFunctionImpl) +OSIM_TEST(FunctionBasedPath, GetLengthIsCached) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + // 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. - SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); - SimTK::State& s = model.initSystem(); + 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(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + SimTK_TEST(fn.m_SharedData->numTimesCalcValueCalled == 0); - { - SimTK::Vector_ bodyForces; - SimTK::Vector mobilityForces; - fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); - } + SimTK::State& s = m.model.initSystem(); + m.model.realizePosition(s); - SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 1); + 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, AddInEquivalentForcesIsNotCached) +OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedUsesUnderlyingFunctionCalcValue) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); - - SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + // the FunctionBasedPath ultimately calculates its lengthening speed + // by using the underlying Function object, rather than computing it + // itself - SimTK::State& s = model.initSystem(); + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); - SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 0); + 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::Vector_ bodyForces; - SimTK::Vector mobilityForces; - fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); - } + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 0); - SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 1); + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); - { - SimTK::Vector_ bodyForces; - SimTK::Vector mobilityForces; - fbp->addInEquivalentForces(s, 1.0, bodyForces, mobilityForces); - } + double lengtheningSpeed = fbp->getLengtheningSpeed(s); - SimTK_TEST(pathFn.data->numTimesAddInEquivalentForcesCalled == 2); + 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, CanCallComputeMomentArmWithoutThrowing) +OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedIsCached) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + // the FunctionBasedPath ultimately calculates its lengthening speed + // by using the underlying Function object, rather than computing it + // itself - OpenSim::Body* body = new OpenSim::Body{}; - body->setMass(1.0); - model.addComponent(body); + ModelWithNCoordinates m = GenerateModelWithNCoordinates(1); - OpenSim::PinJoint* joint = new OpenSim::PinJoint{}; - joint->connectSocket_parent_frame(model.getGround()); - joint->connectSocket_child_frame(*body); - model.addJoint(joint); + 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::State& s = model.initSystem(); + SimTK_TEST(fn.m_SharedData->numTimesCalcDerivativeCalled == 0); - fbp->computeMomentArm(s, joint->getCoordinate()); + 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, ComputeMomentArmUsesPathFunctionImpl) +OSIM_TEST(FunctionBasedPath, AddInEquivalentForcesOnValidFBPDoesNotThrow) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); - - OpenSim::Body* body = new OpenSim::Body{}; - body->setMass(1.0); - model.addComponent(body); - - OpenSim::PinJoint* joint = new OpenSim::PinJoint{}; - joint->connectSocket_parent_frame(model.getGround()); - joint->connectSocket_child_frame(*body); - model.addJoint(joint); + // calling `addInEquivalentForces` on a fully-initialized function-based + // path shouldn't throw, and should probably add *something* to the mobility + // forces outparam - SimTK_TEST(pathFn.data->numTimesComputeMomentArmCalled == 0); + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + MockPathFunction fn; + fn.m_SharedData->argumentSize = static_cast(m.coordinateAbsPaths.size()); // all coords affect this path - SimTK::State& s = model.initSystem(); + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, m.coordinateAbsPaths}; + m.model.addComponent(fbp); - SimTK_TEST(pathFn.data->numTimesComputeMomentArmCalled == 0); + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); - SimTK_TEST(fbp->computeMomentArm(s, joint->getCoordinate()) == pathFn.data->computeMomentArmValue); + 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 - SimTK_TEST(pathFn.data->numTimesComputeMomentArmCalled == 1); + fbp->addInEquivalentForces(s, tension, bodyForces, mobilityForces); // shouldn't throw } -OSIM_TEST(FunctionBasedPath, PathFunctionExtendConnectToModelIsCalledWhenFinalizeConnectionCalledOnTopLevelModel) +OSIM_TEST(FunctionBasedPath, AddInEquivalentForcesOnEmptyCoordlistWorks) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + // edge-case: you can still add in equivalent forces (i.e. nothing) when + // using an FBP that isn't parameterized by any Coordinates - SimTK_TEST(pathFn.data->numTimesExtendConnectToModelCalled == 0); - model.finalizeConnections(); - SimTK_TEST(pathFn.data->numTimesExtendConnectToModelCalled == 1); -} + ModelWithNCoordinates m = GenerateModelWithNCoordinates(5); + MockPathFunction fn; + fn.m_SharedData->argumentSize = 0; // no coords affect this path -OSIM_TEST(FunctionBasedPath, PathFunctionExtendInitStateFromPropertiesCalledWhenCalledOnTopLevelModel) -{ - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{fn, {}}; + m.model.addComponent(fbp); - SimTK_TEST(pathFn.data->numTimesExtendInitStateFromPropertiesCalled == 0); - model.initSystem(); - SimTK_TEST(pathFn.data->numTimesExtendConnectToModelCalled == 1); -} + SimTK::State& s = m.model.initSystem(); + m.model.realizeAcceleration(s); -OSIM_TEST(FunctionBasedPath, PathFunctionExtendAddToSystemCalledWhenInitSystemCalledOnTopLevelModel) -{ - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); + 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 - SimTK_TEST(pathFn.data->numTimesExtendAddToSystemCalled == 0); - model.initSystem(); - SimTK_TEST(pathFn.data->numTimesExtendAddToSystemCalled == 1); + fbp->addInEquivalentForces(s, tension, bodyForces, mobilityForces); // shouldn't throw } -OSIM_TEST(FunctionBasedPath, PathFunctionExtendFinalizePropertiesCalledWhenFinalizeFromPropertiesCalledOnTopLevelModel) +OSIM_TEST(FunctionBasedPath, ComputeMomentArmReturns0IfCalledWithNonAffectingCoord) { - OpenSim::Model model; - MockPathFunction pathFn; - OpenSim::FunctionBasedPath* fbp = new OpenSim::FunctionBasedPath{pathFn}; - model.addComponent(fbp); // also calls it? + // `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); - SimTK_TEST(pathFn.data->numTimesExtendFinalizeFromPropertiesCalled == 1); - model.finalizeFromProperties(); - SimTK_TEST(pathFn.data->numTimesExtendFinalizeFromPropertiesCalled == 2); + 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 // @@ -737,207 +959,29 @@ namespace joris { } }; - class FunctionBasedPathDiscretizationSet : public OpenSim::Set { - OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathDiscretizationSet, OpenSim::Set); - }; - - class JorisFBP : public OpenSim::PathFunction { - OpenSim_DECLARE_CONCRETE_OBJECT(JorisFBP, OpenSim::PathFunction); - - public: - OpenSim_DECLARE_UNNAMED_PROPERTY(FunctionBasedPathDiscretizationSet, "Discretizations that were used for each OpenSim::Coordinate that the path was fitted against"); - OpenSim_DECLARE_LIST_PROPERTY(Evaluations, double, "The evaluated results of each *permutation* of discretizations. The FunctionBasedPathDiscretizationSet property describes how each OpenSim::Coordinate was discretized. These evaluations are the result of permuting through all possible combinations of discretizations. Effectively, this property contains a N-dimensional 'surface' of points, where each dimension of the surface is a Coordinate, and each dimension of each point is one of the evenly-spaced points in the discretization range [x_begin, x_range] for each dimension"); - - // direct pointers to each coordinate - std::vector coords; - - // absolute paths of each coordinate (1:1 with coords) - std::vector coordAbsPaths; - - // discretizations ranges for each coordinate (1:1 with coords) - std::vector discretizations; - - // evaluations for each permutation of coordinates' discretizations - std::vector evals; - - JorisFBP(); - double getLength(const SimTK::State&) const override; - double getLengtheningSpeed(const SimTK::State&) const override; - double computeMomentArm(const SimTK::State&, const OpenSim::Coordinate&) const override; - void addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_& bodyForces, SimTK::Vector& mobilityForces) const override; - void extendFinalizeFromProperties() override; - void extendFinalizeConnections(OpenSim::Component&) override; - }; - - struct FittingParams final { + // returns interpolated path length for a given permutation of coordinate + // values + // + // this is the "heart" of the FPB algorithm. It's loosely based on the algorithm + // described here: + // + // "Two hierarchies of spline interpolations. Practical algorithms for multivariate higher order splines" + // https://arxiv.org/abs/0905.3564 + // + // `inputVals` points to a sequence of `nCoords` values that were probably + // retrieved via `Coordinate::getValue(SimTK::State const&)`. The reason + // that `inputVals` is provided externally (rather than have this implementation + // handle calling `getValue`) is because derivative calculations need to fiddle + // the input values slightly + double Impl_GetPathLength( + const std::vector& discretizations, + const std::vector& evals, + const SimTK::Vector& coordVals) + { + SimTK_ASSERT_ALWAYS(!discretizations.empty(), "FBPs require at least one coordinate to affect the path"); + SimTK_ASSERT_ALWAYS(coordVals.size() == static_cast(discretizations.size()), "You must call this function with the correct number of (precomputed) coordinate values"); - // 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} - { - } - }; - - // 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( - JorisFBP& impl, - const OpenSim::Model& model, - const OpenSim::PointBasedPath& pbp, - const FittingParams& params) - { - // copy model, so we can independently equilibrate + realize + modify the - // copy without having to touch the source model - std::unique_ptr modelClone{model.clone()}; - SimTK::State& initialState = modelClone->initSystem(); - modelClone->equilibrateMuscles(initialState); - modelClone->realizeVelocity(initialState); - - // set `coords` - impl.coords = coordsThatAffectPBP(*modelClone, pbp, initialState, params.numProbingDiscretizations, params.minProbingMomentArmChange); - if (static_cast(impl.coords.size()) > params.maxCoordsThatCanAffectPath || impl.coords.size() == 0) { - impl.coords.clear(); - return false; - } - - // set `coordAbsPaths` - impl.coordAbsPaths.clear(); - impl.coordAbsPaths.reserve(impl.coords.size()); - for (const OpenSim::Coordinate* c : impl.coords) { - impl.coordAbsPaths.push_back(c->getAbsolutePathString()); - } - - // set `discretizations` - impl.discretizations.clear(); - impl.discretizations.reserve(impl.coords.size()); - for (const OpenSim::Coordinate* c : impl.coords) { - impl.discretizations.push_back(discretizationForCoord(*c, params.numDiscretizationStepsPerDimension)); - } - - // set `evals` - SimTK_ASSERT_ALWAYS(impl.coords.size() == impl.discretizations.size(), "these should be equal by now"); - impl.evals = computeEvaluationsFromPBP(pbp, initialState, impl.coords.data(), impl.discretizations.data(), impl.coords.size()); - - return true; - } - - // init underlying implementation data from a `FunctionBasedPath`s (precomputed) properties - // - // the properties being set in the FBP usually implies that the FBP has already been built - // from a PBP at some previous point in time - 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)); - } - } - - // 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]); - } - } - - // returns interpolated path length for a given permutation of coordinate - // values - // - // this is the "heart" of the FPB algorithm. It's loosely based on the algorithm - // described here: - // - // "Two hierarchies of spline interpolations. Practical algorithms for multivariate higher order splines" - // https://arxiv.org/abs/0905.3564 - // - // `inputVals` points to a sequence of `nCoords` values that were probably - // retrieved via `Coordinate::getValue(SimTK::State const&)`. The reason - // that `inputVals` is provided externally (rather than have this implementation - // handle calling `getValue`) is because derivative calculations need to fiddle - // the input values slightly - double Impl_GetPathLength(JorisFBP const& impl, - double const* inputVals, - int nCoords) - { - SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); - SimTK_ASSERT_ALWAYS(nCoords == static_cast(impl.coords.size()), "You must call this function with the correct number of (precomputed) coordinate values"); - - // compute: + // compute: // // - the index of the first discretization step *before* the input value // @@ -946,9 +990,9 @@ namespace joris { using Polynomial = std::array; std::array closestDiscretizationSteps; std::array betas; - for (int coord = 0; coord < nCoords; ++coord) { - double inputVal = inputVals[coord]; - Discretization const& disc = impl.discretizations[coord]; + for (int coord = 0; coord < coordVals.size(); ++coord) { + double inputVal = coordVals[coord]; + Discretization const& disc = discretizations[coord]; double step = (disc.end - disc.begin) / (disc.nsteps - 1); // compute index of first discretization step *before* the input value and @@ -1005,7 +1049,7 @@ namespace joris { // // the algorithm increments this array as it goes through each permutation std::array dimIdxOffsets; - for (int coord = 0; coord < nCoords; ++coord) { + for (int coord = 0; coord < coordVals.size(); ++coord) { dimIdxOffsets[coord] = -1; } @@ -1040,25 +1084,25 @@ namespace joris { // go backwards, from least-significant coordinate (highest idx) // // this is so that we can compute the stride as the algorithm runs - for (int coord = nCoords-1; coord >= 0; --coord) { + for (int coord = coordVals.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]; evalIdx += evalStride * step; - evalStride *= impl.discretizations[coord].nsteps; + evalStride *= discretizations[coord].nsteps; } // equivalent to z += b*v, but handles rounding errors when the rhs // is very small - z = std::fma(beta, impl.evals.at(evalIdx), z); + z = std::fma(beta, evals.at(evalIdx), z); // increment the offsets // // this is effectively the step that permutes [-1, -1, 2] --> [-1, 0, -1] { - int pos = nCoords-1; + int pos = coordVals.size()-1; ++dimIdxOffsets[pos]; // perform least-significant increment (may overflow) while (pos > 0 && dimIdxOffsets[pos] > 2) { // handle overflows + carry propagation dimIdxOffsets[pos] = -1; // overflow @@ -1072,7 +1116,7 @@ namespace joris { // sanity check: is `z` accumulated from the expected number of iterations? { - int expectedIterations = 1 << (2*nCoords); + int expectedIterations = 1 << (2*coordVals.size()); if (cnt != expectedIterations) { std::stringstream msg; msg << "invalid number of permutations explored: expected = " << expectedIterations << ", got = " << cnt; @@ -1083,65 +1127,18 @@ namespace joris { return z; } - // get the length of the path in the current state - double Impl_GetPathLength(JorisFBP const& impl, SimTK::State const& s) - { - int nCoords = static_cast(impl.coords.size()); - - // get the input value of each coordinate in the current state - std::array inputVals{}; - for (int coord = 0; coord < nCoords; ++coord) { - inputVals[coord] = impl.coords[coord]->getValue(s); - } - - return Impl_GetPathLength(impl, inputVals.data(), nCoords); - } - // get the *derivative* of the path length with respect to the given Coordinate index // (in impl.coords) - //static double Impl_GetPathLengthDerivative(OpenSim::FunctionBasedPath::Impl const& impl, - // SimTK::State const& s, - // int coordIdx) { - - // SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); - // SimTK_ASSERT_ALWAYS(coordIdx != -1, "coord index must be valid"); - // SimTK_ASSERT_ALWAYS(coordIdx < static_cast(impl.coords.size()), "coord index must be valid"); - - // int nCoords = static_cast(impl.coords.size()); - - // // get the input value of each coordinate in the current state - // std::array inputVals{}; - // for (int coord = 0; coord < nCoords; ++coord) { - // inputVals[coord] = impl.coords[coord]->getValue(s); - // } - - // // compute value at current point - // double v1 = Impl_GetPathLength(impl, inputVals.data(), nCoords); - - // static constexpr double h = 0.00001; - - // // alter the input value for the to-be-derived coordinate *slightly* and recompute - // inputVals[coordIdx] += h; - // double v2 = Impl_GetPathLength(impl, inputVals.data(), nCoords); - - // // the derivative is how much the output changed when the input was altered - // // slightly (this is a poor-man's discrete derivative method) - // return (v2 - v1) / h; - //} - double Impl_GetPathLengthDerivative(JorisFBP const& impl, - SimTK::State const& s, - int coordIdx) + double Impl_GetPathLengthDerivative( + const std::vector& discretizations, + const std::vector& evals, + const SimTK::Vector& coordVals, + int coordIdx) { - int nCoords = static_cast(impl.coords.size()); - - SimTK_ASSERT_ALWAYS(!impl.coords.empty(), "FBPs require at least one coordinate to affect the path"); - SimTK_ASSERT_ALWAYS(nCoords == static_cast(impl.coords.size()), "You must call this function with the correct number of (precomputed) coordinate values"); + int nCoords = static_cast(coordVals.size()); - // get the input value of each coordinate in the current state - std::array inputVals{}; - for (int coord = 0; coord < nCoords; ++coord) { - inputVals[coord] = impl.coords[coord]->getValue(s); - } + SimTK_ASSERT_ALWAYS(!discretizations.empty(), "FBPs require at least one coordinate to affect the path"); + SimTK_ASSERT_ALWAYS(nCoords == static_cast(discretizations.size()), "You must call this function with the correct number of (precomputed) coordinate values"); // compute: // @@ -1153,8 +1150,8 @@ namespace joris { std::array closestDiscretizationSteps; std::array betas; for (int coord = 0; coord < nCoords; ++coord) { - double inputVal = inputVals[coord]; - Discretization const& disc = impl.discretizations[coord]; + double inputVal = coordVals[coord]; + Discretization const& disc = discretizations[coord]; double step = (disc.end - disc.begin) / (disc.nsteps - 1); // compute index of first discretization step *before* the input value and @@ -1223,11 +1220,11 @@ namespace joris { beta *= betas[coord][offset+1]; evalIdx += evalStride * step; - evalStride *= impl.discretizations[coord].nsteps; + evalStride *= discretizations[coord].nsteps; } - double gridSize = (impl.discretizations[coordIdx].end-impl.discretizations[coordIdx].begin)/impl.discretizations[coordIdx].nsteps; - z = std::fma(beta, impl.evals.at(evalIdx)/gridSize, z); + double gridSize = (discretizations[coordIdx].end-discretizations[coordIdx].begin)/discretizations[coordIdx].nsteps; + z = std::fma(beta, evals.at(evalIdx)/gridSize, z); { int pos = nCoords-1; ++dimIdxOffsets[pos]; // perform least-significant increment (may overflow) @@ -1252,48 +1249,220 @@ namespace joris { return z; } - // get the *derivative* of the path length with respect to the given Coordinate - double Impl_GetPathLengthDerivative(JorisFBP const& impl, - SimTK::State const& s, - OpenSim::Coordinate const& c) + class FunctionBasedPathDiscretizationSet : public OpenSim::Set { + OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathDiscretizationSet, OpenSim::Set); + }; + + 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_GetPathLength(*_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_GetPathLengthDerivative(*_discretizations, *_evaluations, coordVals, derivComponents[0]); + } + + int getArgumentSize() const override + { + return static_cast(_discretizations->size()); + } + + int getMaxDerivativeOrder() const override + { + return 1; + } + }; + + 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}; + } + }; + + 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: handle computing a fresh FBP from a PBP, flashing from props, etc. + + // 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) { - // figure out the index of the coordinate being referred to - int coordIdx = -1; - for (int i = 0; i < static_cast(impl.coords.size()); ++i) { - if (impl.coords[i] == &c) { - coordIdx = i; - break; - } + // 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; } - // ensure the coordinate was actually found, or this alg will break - if (coordIdx == -1) { - std::stringstream msg; - msg << "could not find coordiante '" << c.getName() << "' in the set of coordinates the FunctionBasedPath handles. Coordinates handled by this path are: "; - char const* delim = ""; - for (OpenSim::Coordinate const* c : impl.coords) { - msg << delim << c->getName(); - delim = ", "; - } - OPENSIM_THROW(OpenSim::Exception, std::move(msg).str()); + // set `coordAbsPaths` + impl.coordAbsPaths.clear(); + impl.coordAbsPaths.reserve(impl.coords.size()); + for (const OpenSim::Coordinate* c : impl.coords) { + impl.coordAbsPaths.push_back(c->getAbsolutePathString()); } - // use the "raw" (non-lookup) version of this function with the index - return Impl_GetPathLengthDerivative(impl, s, coordIdx); + // 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; } - // get the lengthening speed of the path in the current state - double Impl_GetLengtheningSpeed(const JorisFBP& impl, - const SimTK::State& state) + // 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) { - double lengtheningSpeed = 0.0; - for (int coordIdx = 0; coordIdx < static_cast(impl.coords.size()); ++coordIdx) { - double deriv = Impl_GetPathLengthDerivative(impl, state, coordIdx); - double coordSpeedVal = impl.coords[coordIdx]->getSpeedValue(state); + 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); + } - lengtheningSpeed = std::fma(deriv, coordSpeedVal, lengtheningSpeed); + // 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)); + } + } + + // 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]); } - return lengtheningSpeed; } std::unique_ptr fromPointBasedPath( @@ -1354,78 +1523,33 @@ namespace joris { return fbp; } + */ +} - JorisFBP::JorisFBP() - { - constructProperty_FunctionBasedPathDiscretizationSet(FunctionBasedPathDiscretizationSet{}); - constructProperty_Evaluations(); - } - - double JorisFBP::getLength(const SimTK::State& s) const - { - return Impl_GetPathLength(*this, s); - } - - double JorisFBP::getLengtheningSpeed(const SimTK::State& s) const +// TODO: test Joris's implementation with analytic functions etc. +namespace joris { + OSIM_TEST(JorisFBP, CanBeDefaultConstructed) { - return Impl_GetLengtheningSpeed(*this, s); + JorisPathFunction fn; } - double JorisFBP::computeMomentArm(const SimTK::State& s, const OpenSim::Coordinate& aCoord) const + OSIM_TEST(JorisFBP, CanBeUsedInAFunctionBasedPath) { - return Impl_GetPathLengthDerivative(*this, s, aCoord); + JorisPathFunction fn; + OpenSim::FunctionBasedPath{fn, {}}; // trivial-case } - void JorisFBP::addInEquivalentForces(const SimTK::State& state, double tension, SimTK::Vector_&, SimTK::Vector& mobilityForces) const + OSIM_TEST(JorisFBP, HasDerivativeOrderGreaterThanOrEqualTo1) { - const SimTK::SimbodyMatterSubsystem& matter = getModel().getMatterSubsystem(); + // the path function should be differentiable - for (const OpenSim::Coordinate* coord : coords) { - double ma = computeMomentArm(state, *coord); - double torqueOverCoord = -tension*ma; - - matter.addInMobilityForce(state, - SimTK::MobilizedBodyIndex(coord->getBodyIndex()), - SimTK::MobilizerUIndex(coord->getMobilizerQIndex()), - torqueOverCoord, - mobilityForces); - } - } - - void JorisFBP::extendFinalizeFromProperties() - { - Impl_InitFromFBPProperties(*this); - } - - void JorisFBP::extendFinalizeConnections(OpenSim::Component& root) - { - // populate pointer-based coordinate lookups - // - // the reason this isn't done in `extendFinalizeFromProperties` is because the - // not-yet-property-finalized Model hasn't necessarily "connected" to the - // coordinates that the coordinate files refer to, so the implementation - // can't lookup the `OpenSim::Coordinate*` pointers during that phase - - // Allow (model) component to include its own subcomponents - // before calling the base method which automatically invokes - // connect all the subcomponents. - { - Model* model = dynamic_cast(&root); - if (model) { - connectToModel(*model); - } - } - - Impl_SetCoordinatePointersFromCoordinatePaths(*this, root); - } - - OSIM_TEST(JorisFBP, CanBeDefaultConstructed) - { - JorisFBP fbp; + JorisPathFunction fn; + SimTK_TEST(fn.getMaxDerivativeOrder() >= 1) } - // TODO: port `Applications/FunctionBasedPathConversion/test/testFunctionBasedPathConversion.cpp` - // see: `https://github.com/joris997/opensim-core/tree/interpolation/Applications/FunctionBasedPathConversion` + // etc.: maybe these tests can be broken down into lower-level and higher + // -level (e.g. test low-level polynomial maths, test their use in + // a higher-level function object, etc. etc.) } int main() From 010ff0cf03cebf5a1a400f27f69267fb7444fd88 Mon Sep 17 00:00:00 2001 From: Adam Kewley Date: Mon, 6 Dec 2021 12:20:22 +0100 Subject: [PATCH 11/12] Refactored Joris's code into low-level, OpenSim, and 'fitting' layers --- .../Simulation/Model/FunctionBasedPath.cpp | 14 + .../Simulation/Test/testFunctionBasedPath.cpp | 960 +++++++++--------- 2 files changed, 511 insertions(+), 463 deletions(-) diff --git a/OpenSim/Simulation/Model/FunctionBasedPath.cpp b/OpenSim/Simulation/Model/FunctionBasedPath.cpp index fc37dec43d..1ba07312f8 100644 --- a/OpenSim/Simulation/Model/FunctionBasedPath.cpp +++ b/OpenSim/Simulation/Model/FunctionBasedPath.cpp @@ -305,3 +305,17 @@ int OpenSim::FunctionBasedPath::indexOfCoordinate(const Coordinate& c) const } 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/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp index c8e3d54333..98e0685d05 100644 --- a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -631,9 +631,11 @@ OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedUsesUnderlyingFunctionCalcValue) OSIM_TEST(FunctionBasedPath, GetLengtheningSpeedIsCached) { - // the FunctionBasedPath ultimately calculates its lengthening speed - // by using the underlying Function object, rather than computing it - // itself + // 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); @@ -751,216 +753,106 @@ OSIM_TEST(FunctionBasedPath, ComputeMomentArmCallsCalcDerivOfFunction) SimTK_TEST(momentArm == fn.m_SharedData->derivative); } -// HACK: test Joris's implementation here -// -// this is because I cba'd splitting OSIM_TEST into another compilation unit, and because -// it's currently handy to have all this code in one unit while we flesh out the implementation - -#include - -namespace joris { - static constexpr size_t g_MaxCoordsThatCanBeInterpolated = 8; // important: this is an upper limit that's used for stack allocations - static constexpr int g_MaxCoordsThatCanAffectPathDefault = static_cast(g_MaxCoordsThatCanBeInterpolated); - static constexpr int g_NumProbingDiscretizationsDefault = 8; - static constexpr double g_MinProbingMomentArmChangeDefault = 0.001; - static constexpr int g_NumDiscretizationStepsPerDimensionDefault = 8; - - // returns `true` if changing the supplied `Coordinate` changes the moment arm - // of the supplied `PointBasedPath` (PBP) - 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); +// HACK: test Joris's implementation here - return affectsCoord; - } +#include - // 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; - } +// core runtime algorithm +namespace joris { - rv.push_back(&c); - } - return rv; - } + static constexpr size_t g_MaxNumDimensions = 8; // important: this is an upper limit that's used for stack allocations - // discretization of a particular coordinate + // an `nPoints` evenly-spaced discretization of the range [begin, end] (inclusive) // - // assumes `nsteps` evenly-spaced points ranging from [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 nsteps; + int nPoints; }; - // compute ideal discretization of the given coordinate - Discretization discretizationForCoord(OpenSim::Coordinate const& c, int numDiscretizationSteps) + double stepSize(const Discretization& d) { - SimTK_ASSERT_ALWAYS(numDiscretizationSteps >= 4, "need to supply more than 4 discretization steps"); - - Discretization d; - //d.begin = -static_cast(SimTK_PI)/2; - //d.end = static_cast(SimTK_PI)/2; - d.begin = std::max(c.getRangeMin(), -static_cast(SimTK_PI)); - d.end = std::min(c.getRangeMax(), static_cast(SimTK_PI)); - d.nsteps = numDiscretizationSteps - 3; - double step = (d.end-d.begin) / (d.nsteps-1); - - // expand range slightly in either direction to ensure interpolation is - // clean around the edges - d.begin -= step; - d.end += 2.0 * step; - d.nsteps += 3; - - return d; + double diff = d.end - d.begin; + return d.nPoints <= 2 ? diff : diff/(d.nPoints-1); } - // 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) + double realIndexOf(const Discretization& d, double v) { - std::vector rv; - - if (ncoords == 0) { // edge-case: logic below assumes ncoords > 0 - return rv; - } + // solve: `x = d.begin + stepSize(d)*n` for `n` + return (v - d.begin) / stepSize(d); + } - OPENSIM_THROW_IF(ncoords > g_MaxCoordsThatCanBeInterpolated, OpenSim::Exception, "too many coordinates affect this path - the FunctionBasedPath implementation cannot handle this"); + std::pair splitIntoWholeAndFractional(double v) + { + double wholePart; + double fractionalPart = std::modf(v, &wholePart); + return {static_cast(wholePart), fractionalPart}; + } - // number of evaluations is the total number of permutations of all dimensions for - // all discretizations - int expectedEvals = 1; - for (size_t i = 0; i < ncoords; ++i) { - expectedEvals *= discs[i].nsteps; + 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; } - rv.reserve(expectedEvals); - - // holds which "step" in each Coordinate's [begin, end] discretization we - // have evaluated up to - std::array discStepIdx{}; - while (discStepIdx[0] < discs[0].nsteps) { - - // set all coordinate values for this step - for (size_t coord = 0; coord < ncoords; ++coord) { - Discretization const& discr = discs[coord]; - - double stepSz = (discr.end - discr.begin) / (discr.nsteps - 1); - int step = discStepIdx[coord]; - double val = discr.begin + step*stepSz; + } - coords[coord]->setValue(state, val); - } + using Polynomial = std::array; - // eval the length of the PBP for this permutation of coordinate values - { - double eval = pbp.getLength(state); - rv.push_back(eval); - } + Polynomial ComputeBeta(double frac) + { + // compute polynomial based on fraction the point is toward the next point - // update which coordinate steps we're up to for each coordinate - // - // always updates "least significant" coordinate first, then performs - // "carry propagation" to the "more significant" coordinates - int pos = ncoords - 1; - discStepIdx[pos]++; - while (pos > 0 && discStepIdx[pos] >= discs[pos].nsteps) { - discStepIdx[pos] = 0; // overflow - ++discStepIdx[pos-1]; // carry - --pos; - } - } + double frac2 = frac*frac; + double frac3 = frac2*frac; + double frac4 = frac3*frac; + double fracMinusOne = frac - 1; + double fracMinusOne3 = fracMinusOne*fracMinusOne*fracMinusOne; - SimTK_ASSERT_ALWAYS(discStepIdx[0] == discs[0].nsteps, "should be true, after the final overflow"); - for (size_t i = 1; i < discStepIdx.size(); ++i) { - SimTK_ASSERT_ALWAYS(discStepIdx[i] == 0, "these less-significant coordinates should all be overflow-n by the end of the alg"); - } - SimTK_ASSERT_ALWAYS(rv.size() == static_cast(expectedEvals), "these two values should match, given the above alg"); + 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 rv; + return p; } - 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); - } - }; + 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; + } - // returns interpolated path length for a given permutation of coordinate - // values + // 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: @@ -968,18 +860,15 @@ namespace joris { // "Two hierarchies of spline interpolations. Practical algorithms for multivariate higher order splines" // https://arxiv.org/abs/0905.3564 // - // `inputVals` points to a sequence of `nCoords` values that were probably - // retrieved via `Coordinate::getValue(SimTK::State const&)`. The reason - // that `inputVals` is provided externally (rather than have this implementation - // handle calling `getValue`) is because derivative calculations need to fiddle - // the input values slightly - double Impl_GetPathLength( - const std::vector& discretizations, - const std::vector& evals, - const SimTK::Vector& coordVals) + // `xVals` are the current values of each dimension (independent var: e.g. coordinate values - the "x"es) + double Impl_ComputeValue( + const std::vector& fittingDiscretizations, + const std::vector& fittingEvals, + const SimTK::Vector& xVals) { - SimTK_ASSERT_ALWAYS(!discretizations.empty(), "FBPs require at least one coordinate to affect the path"); - SimTK_ASSERT_ALWAYS(coordVals.size() == static_cast(discretizations.size()), "You must call this function with the correct number of (precomputed) coordinate values"); + 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: // @@ -987,52 +876,15 @@ namespace joris { // // - the polynomial of the curve at that step, given its fractional distance // toward the next step - using Polynomial = std::array; - std::array closestDiscretizationSteps; - std::array betas; - for (int coord = 0; coord < coordVals.size(); ++coord) { - double inputVal = coordVals[coord]; - Discretization const& disc = discretizations[coord]; - double step = (disc.end - disc.begin) / (disc.nsteps - 1); - - // compute index of first discretization step *before* the input value and - // the fraction that the input value is towards the *next* discretization step - int idx; - double frac; - if (inputVal < disc.begin+step) { - idx = 1; - frac = 0.0; - } else if (inputVal > disc.end-2*step) { - idx = disc.nsteps-3; - frac = 0.0; - } else { - // solve for `n`: inputVal = begin + n*step - double n = (inputVal - disc.begin) / step; - double wholePart; - double fractionalPart = std::modf(n, &wholePart); - - idx = static_cast(wholePart); - frac = fractionalPart; - } - - // compute polynomial based on fraction the point is toward the next point - double frac2 = frac*frac; - double frac3 = frac2*frac; - double frac4 = frac3*frac; - double fracMinusOne = frac - 1; - double fracMinusOne3 = fracMinusOne*fracMinusOne*fracMinusOne; - - Polynomial p; - p[0] = 0.5 * fracMinusOne3*frac*(2*frac + 1); - p[1] = -0.5 * (frac - 1)*(6*frac4 - 9*frac3 + 2*frac + 2); - p[2] = 0.5 * frac*(6*frac4 - 15*frac3 + 9*frac2 + frac + 1); - p[3] = -0.5 * (frac - 1)*frac3*(2*frac - 3); - - closestDiscretizationSteps[coord] = idx; - betas[coord] = p; + 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] = ComputeBeta(wholeFrac.second); } - // for each coord, permute through 4 locations *around* the input's location: + // 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 @@ -1044,18 +896,18 @@ namespace joris { // - betas are coefficients that affect each location. beta[0] affects A, // betas[1] affects B, betas[2] affects C, and betas[3] affects D - // represent permuting through each location around each coordinate as a string + // 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 coord = 0; coord < coordVals.size(); ++coord) { - dimIdxOffsets[coord] = -1; + 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 coords iterate like this for each + // e.g. the location permutations for 3 dims iterate like this for each // crank of the loop // // [-1, -1, -1] @@ -1068,41 +920,42 @@ namespace joris { // [ 2, 2, 2] // [ 3, 0, 0] (the termination condition) - double z = 0.0; - int cnt = 0; + double z = 0.0; // result + int cnt = 0; // sanity-check counter while (dimIdxOffsets[0] < 3) { - // compute `beta` (weighted coefficient per coord) for this particular - // permutation's coordinate locations (e.g. -1, 0, 0, 2) and figure out + // 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 evalStride = 1; - int evalIdx = 0; + int strideInFittingEvals = 1; + int idxInFittingEvals = 0; - // go backwards, from least-significant coordinate (highest idx) + // 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 = coordVals.size()-1; coord >= 0; --coord) { + 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]; - evalIdx += evalStride * step; - evalStride *= discretizations[coord].nsteps; + 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, evals.at(evalIdx), z); + z = std::fma(beta, fittingEvals.at(idxInFittingEvals), z); // increment the offsets // // this is effectively the step that permutes [-1, -1, 2] --> [-1, 0, -1] { - int pos = coordVals.size()-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 @@ -1116,7 +969,7 @@ namespace joris { // sanity check: is `z` accumulated from the expected number of iterations? { - int expectedIterations = 1 << (2*coordVals.size()); + int expectedIterations = 1 << (2*xVals.size()); if (cnt != expectedIterations) { std::stringstream msg; msg << "invalid number of permutations explored: expected = " << expectedIterations << ", got = " << cnt; @@ -1127,18 +980,16 @@ namespace joris { return z; } - // get the *derivative* of the path length with respect to the given Coordinate index - // (in impl.coords) - double Impl_GetPathLengthDerivative( - const std::vector& discretizations, - const std::vector& evals, - const SimTK::Vector& coordVals, - int coordIdx) + // computes the Y derivative for a given petmuation of input X values + double Impl_ComputeDeriv( + const std::vector& fittingDiscretizations, + const std::vector& fittingEvals, + const SimTK::Vector& xVals, + int dimToDifferentiateIdx) { - int nCoords = static_cast(coordVals.size()); - - SimTK_ASSERT_ALWAYS(!discretizations.empty(), "FBPs require at least one coordinate to affect the path"); - SimTK_ASSERT_ALWAYS(nCoords == static_cast(discretizations.size()), "You must call this function with the correct number of (precomputed) coordinate values"); + SimTK_ASSERT_ALWAYS(!fittingDiscretizations.empty(), "this implementation requires that at least one X (e.g. coordinate) dimension has been fitted"); + SimTK_ASSERT_ALWAYS(xVals.size() == static_cast(fittingDiscretizations.size()), "You must call this function with the correct number of (precomputed) x values (i.e. the same number that were fitted against)"); + SimTK_ASSERT_ALWAYS(xVals.size() < static_cast(g_MaxNumDimensions), "too many dimensions in this fit - the implementation cannot handle this many"); // compute: // @@ -1147,86 +998,70 @@ namespace joris { // - the polynomial of the curve at that step, given its fractional distance // toward the next step using Polynomial = std::array; - std::array closestDiscretizationSteps; - std::array betas; - for (int coord = 0; coord < nCoords; ++coord) { - double inputVal = coordVals[coord]; - Discretization const& disc = discretizations[coord]; - double step = (disc.end - disc.begin) / (disc.nsteps - 1); - - // compute index of first discretization step *before* the input value and - // the fraction that the input value is towards the *next* discretization step - int idx; - double frac; - if (inputVal < disc.begin+step) { - idx = 1; - frac = 0.0; - } else if (inputVal > disc.end-2*step) { - idx = disc.nsteps-3; - frac = 0.0; - } else { - // solve for `n`: inputVal = begin + n*step - double n = (inputVal - disc.begin) / step; - double wholePart; - double fractionalPart = std::modf(n, &wholePart); - - idx = static_cast(wholePart); - frac = fractionalPart; - } - - // compute polynomial based on fraction the point is toward the next point - double frac2 = frac*frac; - double frac3 = frac2*frac; - double frac4 = frac3*frac; - double fracMinusOne = frac - 1; - double fracMinusOne3 = fracMinusOne*fracMinusOne*fracMinusOne; - - Polynomial p; - if (coord == coordIdx){ - // derivative - p[0] = 5*frac4 - 10*frac3 + 4.5*frac2 + frac - 0.5; - p[1] = -15*frac4 + 30*frac3 - 13.5*frac2 - 2*frac; - p[2] = 15*frac4 - 30*frac3 + 13.5*frac2 + frac + 0.5; - p[3] = frac2*(-5*frac2 + 10*frac - 4.5); - } else { - // 'normal' spline function - p[0] = 0.5 * fracMinusOne3*frac*(2*frac + 1); - p[1] = -0.5 * (frac - 1)*(6*frac4 - 9*frac3 + 2*frac + 2); - p[2] = 0.5 * frac*(6*frac4 - 15*frac3 + 9*frac2 + frac + 1); - p[3] = -0.5 * (frac - 1)*frac3*(2*frac - 3); - } - - closestDiscretizationSteps[coord] = idx; - betas[coord] = p; + std::array 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 != dimToDifferentiateIdx ? ComputeBeta(wholeFrac.second) : ComputeBetaDerivative(wholeFrac.second); } - std::array dimIdxOffsets; - for (int coord = 0; coord < nCoords; ++coord) { - dimIdxOffsets[coord] = -1; + std::array dimIdxOffsets; + for (int dim = 0; dim < xVals.size(); ++dim) { + dimIdxOffsets[dim] = -1; } - double z = 0.0; - int cnt = 0; + // 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 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 evalStride = 1; - int evalIdx = 0; + int strideInFittingEvals = 1; + int idxInFittingEvals = 0; - for (int coord = nCoords-1; coord >= 0; --coord) { + // 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]; - evalIdx += evalStride * step; - evalStride *= discretizations[coord].nsteps; + beta *= betas[coord][offset+1]; // index into the polynomial + idxInFittingEvals += strideInFittingEvals * step; + strideInFittingEvals *= fittingDiscretizations[coord].nPoints; } - double gridSize = (discretizations[coordIdx].end-discretizations[coordIdx].begin)/discretizations[coordIdx].nsteps; - z = std::fma(beta, evals.at(evalIdx)/gridSize, z); + // equivalent to z += b*v, but handles rounding errors when the rhs + // is very small + z = std::fma(beta, fittingEvals.at(idxInFittingEvals), z); + + // increment the offsets + // + // this is effectively the step that permutes [-1, -1, 2] --> [-1, 0, -1] { - int pos = nCoords-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 @@ -1237,8 +1072,10 @@ namespace joris { ++cnt; } + + // sanity check: is `z` accumulated from the expected number of iterations? { - int expectedIterations = 1 << (2*nCoords); + int expectedIterations = 1 << (2*xVals.size()); if (cnt != expectedIterations) { std::stringstream msg; msg << "invalid number of permutations explored: expected = " << expectedIterations << ", got = " << cnt; @@ -1248,10 +1085,17 @@ namespace joris { return z; } +} + + +// TODO: test the low-level algorithm by providing it low-level data etc +namespace joris { + +} - class FunctionBasedPathDiscretizationSet : public OpenSim::Set { - OpenSim_DECLARE_CONCRETE_OBJECT(FunctionBasedPathDiscretizationSet, OpenSim::Set); - }; + +// wireup of core runtime algorithm to OpenSim +namespace joris { class JorisPathSimTKFunction : public SimTK::Function { std::shared_ptr> _discretizations; @@ -1267,13 +1111,13 @@ namespace joris { double calcValue(const SimTK::Vector& coordVals) const override { - return Impl_GetPathLength(*_discretizations, *_evaluations, coordVals); + return Impl_ComputeValue(*_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_GetPathLengthDerivative(*_discretizations, *_evaluations, coordVals, derivComponents[0]); + return Impl_ComputeDeriv(*_discretizations, *_evaluations, coordVals, derivComponents[0]); } int getArgumentSize() const override @@ -1287,6 +1131,30 @@ namespace joris { } }; + 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 @@ -1312,105 +1180,11 @@ namespace joris { { return new JorisPathSimTKFunction{_discretizations, _evaluations}; } + + // TODO: flash vectors from properties }; - 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: handle computing a fresh FBP from a PBP, flashing from props, etc. - - // 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: handle computing a fresh FBP from a PBP, flashing from props, etc. // init underlying implementation data from a `FunctionBasedPath`s (precomputed) properties // @@ -1453,18 +1227,237 @@ namespace joris { 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" that uses Joris's alg +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; + } - // ensure that the OpenSim::Coordinate* pointers held in Impl are up-to-date + // returns a sequence of `OpenSim::Coordinate`s that affect the supplied + // point-based path (PBP) // - // the pointers are there to reduce runtime path lookups - static void Impl_SetCoordinatePointersFromCoordinatePaths(JorisFBP& impl, - OpenSim::Component const& c) { + // 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; + } - for (size_t i = 0; i < impl.coords.size(); ++i) { - impl.coords[i] = &c.getComponent(impl.coordAbsPaths[i]); + 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, @@ -1523,33 +1516,74 @@ namespace joris { return fbp; } - */ -} -// TODO: test Joris's implementation with analytic functions etc. -namespace joris { - OSIM_TEST(JorisFBP, CanBeDefaultConstructed) + // 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) { - JorisPathFunction fn; - } + // 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); - OSIM_TEST(JorisFBP, CanBeUsedInAFunctionBasedPath) - { - JorisPathFunction fn; - OpenSim::FunctionBasedPath{fn, {}}; // trivial-case - } + // 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; + } - OSIM_TEST(JorisFBP, HasDerivativeOrderGreaterThanOrEqualTo1) - { - // the path function should be differentiable + // set `coordAbsPaths` + impl.coordAbsPaths.clear(); + impl.coordAbsPaths.reserve(impl.coords.size()); + for (const OpenSim::Coordinate* c : impl.coords) { + impl.coordAbsPaths.push_back(c->getAbsolutePathString()); + } - JorisPathFunction fn; - SimTK_TEST(fn.getMaxDerivativeOrder() >= 1) + // 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 { +} - // etc.: maybe these tests can be broken down into lower-level and higher - // -level (e.g. test low-level polynomial maths, test their use in - // a higher-level function object, etc. etc.) +// TODO: test conversion tool works as intended +namespace joris { +} + +// TODO: implement CLI tool +namespace joris { +} + +// TODO: implement CLI tests +namespace joris { } int main() From 4cdba3340a8f17bc7e6d093989c14ef400823fc1 Mon Sep 17 00:00:00 2001 From: Adam Kewley Date: Mon, 6 Dec 2021 13:51:03 +0100 Subject: [PATCH 12/12] Merged value and derivative calculations to clarify the differences between them --- .../Simulation/Test/testFunctionBasedPath.cpp | 124 ++---------------- 1 file changed, 12 insertions(+), 112 deletions(-) diff --git a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp index 98e0685d05..5ac4db6f77 100644 --- a/OpenSim/Simulation/Test/testFunctionBasedPath.cpp +++ b/OpenSim/Simulation/Test/testFunctionBasedPath.cpp @@ -861,10 +861,11 @@ namespace joris { // 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_ComputeValue( + double Impl_ComputeValueOrDeriv( const std::vector& fittingDiscretizations, const std::vector& fittingEvals, - const SimTK::Vector& xVals) + 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"); @@ -881,7 +882,7 @@ namespace joris { for (int dim = 0; dim < xVals.size(); ++dim) { std::pair wholeFrac = splitIntoWholeAndFractionalForBetaCalc(fittingDiscretizations[dim], xVals[dim]); closestDiscretizationSteps[dim] = wholeFrac.first; - betas[dim] = ComputeBeta(wholeFrac.second); + betas[dim] = dim != derivIndex ? ComputeBeta(wholeFrac.second) : ComputeBetaDerivative(wholeFrac.second); } // for each dim, permute through 4 (discretized) locations *around* the x location: @@ -922,112 +923,10 @@ namespace joris { double z = 0.0; // result int cnt = 0; // sanity-check counter - 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 + // 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]); - 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, 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; - } - - // computes the Y derivative for a given petmuation of input X values - double Impl_ComputeDeriv( - const std::vector& fittingDiscretizations, - const std::vector& fittingEvals, - const SimTK::Vector& xVals, - int dimToDifferentiateIdx) - { - SimTK_ASSERT_ALWAYS(!fittingDiscretizations.empty(), "this implementation requires that at least one X (e.g. coordinate) dimension has been fitted"); - SimTK_ASSERT_ALWAYS(xVals.size() == static_cast(fittingDiscretizations.size()), "You must call this function with the correct number of (precomputed) x values (i.e. the same number that were fitted against)"); - 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 - using Polynomial = std::array; - 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 != dimToDifferentiateIdx ? ComputeBeta(wholeFrac.second) : ComputeBetaDerivative(wholeFrac.second); - } - - 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 while (dimIdxOffsets[0] < 3) { // compute `beta` (weighted coefficient per dimension) for this particular @@ -1055,7 +954,7 @@ namespace joris { // equivalent to z += b*v, but handles rounding errors when the rhs // is very small - z = std::fma(beta, fittingEvals.at(idxInFittingEvals), z); + z = std::fma(beta, evalScaler * fittingEvals.at(idxInFittingEvals), z); // increment the offsets // @@ -1097,6 +996,7 @@ 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; @@ -1111,13 +1011,13 @@ namespace joris { double calcValue(const SimTK::Vector& coordVals) const override { - return Impl_ComputeValue(*_discretizations, *_evaluations, coordVals); + 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_ComputeDeriv(*_discretizations, *_evaluations, coordVals, derivComponents[0]); + return Impl_ComputeValueOrDeriv(*_discretizations, *_evaluations, coordVals, derivComponents[0]); } int getArgumentSize() const override @@ -1181,7 +1081,7 @@ namespace joris { return new JorisPathSimTKFunction{_discretizations, _evaluations}; } - // TODO: flash vectors from properties + // TODO: flash vectors from properties with `extendFinalizeFromProperties` }; /** TODO: handle computing a fresh FBP from a PBP, flashing from props, etc. @@ -1236,7 +1136,7 @@ namespace joris { } -// TODO: code that compiles a new "FunctionBasedPath" that uses Joris's alg +// 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;