diff --git a/Bindings/Java/Matlab/examples/Wrapping/exampleScholz2015GeometryPath.m b/Bindings/Java/Matlab/examples/Wrapping/exampleScholz2015GeometryPath.m index b8c902d06a..8401dc5609 100644 --- a/Bindings/Java/Matlab/examples/Wrapping/exampleScholz2015GeometryPath.m +++ b/Bindings/Java/Matlab/examples/Wrapping/exampleScholz2015GeometryPath.m @@ -78,6 +78,11 @@ function exampleScholz2015GeometryPath() % adding, it defines the insertion of the path. path.appendPathPoint(model.getBodySet().get('b1'), Vec3(-0.5, 0.1, 0.)); +% Enable warm starts in the wrapping solver. At each time step, the +% wrapping solver will use the solution from the previous time step as an +% initial guess for the current time step. +path.setUseWarmStart(true); + % Initialize the system. state = model.initSystem(); model.updVisualizer().updSimbodyVisualizer().setBackgroundTypeByInt(2); diff --git a/Bindings/Python/examples/Wrapping/exampleScholz2015GeometryPath.py b/Bindings/Python/examples/Wrapping/exampleScholz2015GeometryPath.py index 3ce684c558..e043f2e354 100644 --- a/Bindings/Python/examples/Wrapping/exampleScholz2015GeometryPath.py +++ b/Bindings/Python/examples/Wrapping/exampleScholz2015GeometryPath.py @@ -76,6 +76,11 @@ # adding, it defines the insertion of the path. path.appendPathPoint(model.getBodySet().get('b1'), osim.Vec3(-0.5, 0.1, 0.)) +# Enable warm starts in the wrapping solver. At each time step, the +# wrapping solver will use the solution from the previous time step as an +# initial guess for the current time step. +path.setUseWarmStart(True) + # Initialize the system. state = model.initSystem() model.updVisualizer().updSimbodyVisualizer().setBackgroundTypeByInt(2) diff --git a/CHANGELOG.md b/CHANGELOG.md index c411641ae1..5983d1f050 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -67,6 +67,7 @@ performance and stability in wrapping solutions. - Implemented `extendPreScale` and `extendPostScale` for `Scholz2015GeometryPath`. Now, scaling a model using `Scholz2015GeometryPath` paths will update `Muscle` tendon slack lengths and optimal fiber lengths. (#4325) - Fix crash deserializing xml files that are missing OpenSimDocument tags. (#4336) - Added `exampleExponentialContactForce`, including C++, Matlab, and Python variants. (#4318) +- Added the property `use_warm_start` and accessors `set/getWarmStart()` in `Scholz2015GeometryPath` to enable toggling on and off warm starts, where the wrapper solver from the previous time step as an initial guess for the path at the next time step. By default, `Scholz2015GeometryPath` now has warm starts disabled, meaning that paths are always computed from wrap obstacle contact hints (previously, warm starts were always enabled). (#4342) v4.5.2 diff --git a/OpenSim/Examples/Wrapping/exampleScholz2015GeometryPath.cpp b/OpenSim/Examples/Wrapping/exampleScholz2015GeometryPath.cpp index d62ae40fd2..60b05aefb7 100644 --- a/OpenSim/Examples/Wrapping/exampleScholz2015GeometryPath.cpp +++ b/OpenSim/Examples/Wrapping/exampleScholz2015GeometryPath.cpp @@ -82,6 +82,11 @@ int main() { path.appendPathPoint(model.getComponent("/bodyset/b1"), SimTK::Vec3(-0.5, 0.1, 0.)); + // Enable warm starts in the wrapping solver. At each time step, the + // wrapping solver will use the solution from the previous time step as an + // initial guess for the current time step. + path.setUseWarmStart(true); + // Initialize the system. SimTK::State state = model.initSystem(); model.updVisualizer().updSimbodyVisualizer().setBackgroundType( diff --git a/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp b/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp index 8c292b7d9a..a0ce07a5f0 100644 --- a/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp +++ b/OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp @@ -189,6 +189,17 @@ int Scholz2015GeometryPath::getNumPathElements() const { return getProperty_path_elements().size(); } +void Scholz2015GeometryPath::setUseWarmStart(bool useWarmStart) { + if (_index.isValid()) { + updCableSpan().setUseWarmStart(useWarmStart); + } + set_use_warm_start(useWarmStart); +} + +bool Scholz2015GeometryPath::getUseWarmStart() const { + return get_use_warm_start(); +} + //============================================================================= // ABSTRACT PATH INTERFACE //============================================================================= @@ -363,6 +374,7 @@ void Scholz2015GeometryPath::extendAddToSystem( cable.setCurveSegmentAccuracy(1e-10); cable.setSolverMaxIterations(50); cable.setAlgorithm(SimTK::CableSpanAlgorithm::Scholz2015); + cable.setUseWarmStart(get_use_warm_start()); _index = cable.getIndex(); } @@ -419,6 +431,7 @@ void Scholz2015GeometryPath::extendPostScale(const SimTK::State& s, //============================================================================= void Scholz2015GeometryPath::constructProperties() { constructProperty_path_elements(); + constructProperty_use_warm_start(false); } const Scholz2015GeometryPathObstacle* Scholz2015GeometryPath::getObstacle( diff --git a/OpenSim/Simulation/Model/Scholz2015GeometryPath.h b/OpenSim/Simulation/Model/Scholz2015GeometryPath.h index 162510a86a..b1a6f18957 100644 --- a/OpenSim/Simulation/Model/Scholz2015GeometryPath.h +++ b/OpenSim/Simulation/Model/Scholz2015GeometryPath.h @@ -162,8 +162,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPathObstacle, * can touchdown on the obstacle if the surface obstructs the straight line * segment again. * - * The path is computed as an optimization problem using the previous optimal - * path as the warm start. This is done by computing natural geodesic + * The path is computed as an optimization problem by computing natural geodesic * corrections for each curve segment to compute the locally shortest path, * as described in the following publication: * @@ -172,7 +171,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPathObstacle, * System Dynamics 36, 195–219. * * The overall path is locally the shortest, allowing winding over an obstacle - * multiple times, without flipping to the other side. + * multiple times, without flipping to the other side. The wrapping solver can + * optionally use the solution from the previous time step as a warm start to + * improve performance during forward dynamics simulations (see method + * `getUseWarmStart()` for more information). * * This class encapsulates `SimTK::CableSpan`, the Simbody implementation of * this algorithm. For the full details concerning this class, see the Simbody @@ -425,6 +427,28 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath); */ int getNumPathElements() const; + /** + * Set whether the path uses the solution from the previous time step as a + * warm start for the wrapping solver. + * + * Enable this setting when performing forward dyanmics simulations or any + * simulation where path solutions will be computed sequentially in time + * reasonable small time steps. In other scenarios where the configuration + * of the model, and therefore path solutions, may change rapidly, it is + * recommended to disable this setting to avoid inconsistent path solutions. + * If false, the path will always be computed from the curve contact hints + * when realizing to SimTK::Stage::Position. Default: false. + */ + void setUseWarmStart(bool useWarmStart); + + /** + * Get whether the path uses the solution from the previous time step as a + * warm start for the wrapping solver. + * + * @see setUseWarmStart() + */ + bool getUseWarmStart() const; + // @} //** @name `AbstractGeometryPath` interface */ @@ -446,6 +470,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath); // PROPERTIES OpenSim_DECLARE_LIST_PROPERTY(path_elements, Scholz2015GeometryPathElement, "The list of elements (path points or obstacles) defining the path."); + OpenSim_DECLARE_PROPERTY(use_warm_start, bool, + "Whether to use the path solution from the previous time step as a " + "warm start for the wrapping solver. If false, the path will always be " + "computed from the curve contact hints when realizing to " + "SimTK::Stage::Position. Default: false."); // MODEL COMPONENT INTERFACE void extendConnectToModel(Model& model) override; diff --git a/OpenSim/Tests/Wrapping/testScholz2015GeometryPath.cpp b/OpenSim/Tests/Wrapping/testScholz2015GeometryPath.cpp index 45a098d919..067f571112 100644 --- a/OpenSim/Tests/Wrapping/testScholz2015GeometryPath.cpp +++ b/OpenSim/Tests/Wrapping/testScholz2015GeometryPath.cpp @@ -23,6 +23,7 @@ #include #include +#include #include #include #include @@ -31,6 +32,8 @@ #include #include #include +#include +#include #include @@ -146,6 +149,7 @@ TEST_CASE("Suspended pendulum") { // Set the path's origin and insertion. Scholz2015GeometryPath& path = actu->updPath(); + path.setUseWarmStart(true); path.appendPathPoint(model.getGround(), SimTK::Vec3(-0.1, 0, 0)); // Check that pendulum is at rest with the expected length. @@ -596,6 +600,7 @@ TEST_CASE("Path with all surfaces and a via point") { // Create the path object. Scholz2015GeometryPath* path = new Scholz2015GeometryPath(); + path->setUseWarmStart(true); path->appendPathPoint(*originBody, SimTK::Vec3(0.)); // Add the first torus obstacle. @@ -1014,3 +1019,126 @@ TEST_CASE("Scaling updates muscle properties") { CHECK(scaledMuscle.get_optimal_fiber_length() == 0.123); CHECK(scaledMuscle.get_tendon_slack_length() == 0.123); } + +TEST_CASE("Disabling warm starts unifies moment arm calculations") { + using DataPoints = std::vector>; + + // A simple pendulum model with a path wrapping over an ellipsoid obstacle. + // The model is designed so that, depending on the choice of the default + // value for the 'angle' coordinate, moment arm calculations may differ when + // using the warm start feature of Scholz2015GeometryPath. + auto buildModel = [](bool warmStart, double defaultAngle) { + Model model; + model.setName("pendulum"); + + // Define the pendulum bodies: base and head. + auto* base = new Body("pendulum_base", 1.0, SimTK::Vec3(0), + SimTK::Inertia(1)); + model.addBody(base); + auto* head = new Body("pendulum_head", 1.0, SimTK::Vec3(0), + SimTK::Inertia(1)); + model.addBody(head); + + // Base welded 1.5 m above ground. + auto* weld = new WeldJoint("ground_base", + model.getGround(), SimTK::Vec3(0, 1.5, 0), SimTK::Vec3(0), + *base, SimTK::Vec3(0), SimTK::Vec3(0)); + model.addJoint(weld); + + // Add a PinJoint between the base and head driven by the coordinate + // 'angle'. + auto* pin = new PinJoint("base_to_head", + *base, SimTK::Vec3(0), SimTK::Vec3(0), + *head, SimTK::Vec3(0, 1, 0), SimTK::Vec3(0)); + auto& angle = pin->updCoordinate(); + angle.setName("angle"); + angle.setRangeMin(-0.5); + angle.setRangeMax(0.5); + angle.setDefaultValue(defaultAngle); + model.addJoint(pin); + + // Attach a contact ellipsoid to the base which the muscle path will + // wrap over. + auto* ellipsoid = new ContactEllipsoid( + SimTK::Vec3(0.1, 0.1, 0.5), + SimTK::Vec3(-0.01, -0.25, -0.035), + SimTK::Vec3(0.25, -0.29, 0.075), + *base); + ellipsoid->setName("ellipsoid"); + model.addComponent(ellipsoid); + + // Add muscle with a path between the head and base, wrapping over the + // ellipsoid. + auto* muscle = new Millard2012EquilibriumMuscle(); + muscle->setName("muscle"); + muscle->set_path(Scholz2015GeometryPath()); + model.addForce(muscle); + auto& path = muscle->updPath(); + path.appendPathPoint(*head, SimTK::Vec3(0)); + path.appendObstacle(*ellipsoid, SimTK::Vec3(0.10, 0, 0)); + path.appendPathPoint(*base, SimTK::Vec3(0)); + path.setUseWarmStart(warmStart); + return model; + }; + + // Sweep over the range of motion of the 'angle' coordinate, computing the + // moment arm of the muscle path at each point. This replicates the + // behavior of OpenSim Creator's moment arm plotting functionality, where + // the underlying bug was originally discovered. + auto computeDataPoints = [&](bool warmStart, double defaultAngle) + -> DataPoints { + Model model = buildModel(warmStart, defaultAngle); + SimTK::State state = model.initSystem(); + const Coordinate& coordinate = + model.getComponent("/jointset/base_to_head/angle"); + const Muscle& muscle = model.getComponent("/forceset/muscle"); + const double start = coordinate.getRangeMin(); + const double end = coordinate.getRangeMax(); + SimTK::Vector x = createVectorLinspace(202, start, end); + DataPoints data_points; + for (int i = 0; i < x.size(); ++i) { + coordinate.setValue(state, x[i]); + model.equilibrateMuscles(state); + model.realizeReport(state); + const double y = muscle.getPath().computeMomentArm(state, + coordinate); + data_points.emplace_back(x[i], y); + } + return data_points; + }; + + // Compare two sets of data points sampled at the same coordinate values, + // checking that the moment arm agrees at every point across the range of + // motion. + auto compareDataPoints = [](const DataPoints& lhs, const DataPoints& rhs, + double tolerance, bool useWarmStart = false) { + REQUIRE(lhs.size() == rhs.size()); + SimTK::Vector error(static_cast(lhs.size()), 0.0); + for (int i = 0; i < static_cast(lhs.size()); ++i) { + error[i] = std::abs(lhs[i].second - rhs[i].second); + } + + // If using a warm start, we expect the error to be non-zero (i.e., the + // moment arm curves different) when using different default values for + // 'angle'. + if (useWarmStart) { + CHECK_THAT(error.norm(), + !Catch::Matchers::WithinAbs(0.0, tolerance)); + } else { + CHECK_THAT(error.norm(), + Catch::Matchers::WithinAbs(0.0, tolerance)); + } + + CAPTURE(error); + }; + + // Test that the moment arm curves are identical when using different + // default values for the 'angle' coordinate, when warm starts are disabled. + // Conversely, when warm starts are enabled, the moment arm curves should + // differ depending on the default value of 'angle'. + bool useWarmStart = GENERATE(false, true); + CAPTURE(useWarmStart); + auto lhs = computeDataPoints(useWarmStart, 0.0); + auto rhs = computeDataPoints(useWarmStart, 0.45); + compareDataPoints(lhs, rhs, 1e-6, useWarmStart); +} diff --git a/dependencies/CMakeLists.txt b/dependencies/CMakeLists.txt index 5cc756f98b..feeac76476 100644 --- a/dependencies/CMakeLists.txt +++ b/dependencies/CMakeLists.txt @@ -184,8 +184,8 @@ AddDependency(NAME ezc3d AddDependency(NAME simbody DEFAULT ON - GIT_URL https://github.com/simbody/simbody.git - GIT_TAG 80a3f10a6daa26c8aaf18f2c672cf385b227e62c + GIT_URL https://github.com/nickbianco/simbody.git + GIT_TAG cable_span_warm_start CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF -DBUILD_TESTING:BOOL=OFF ${SIMBODY_EXTRA_CMAKE_ARGS})