Skip to content

Commit e5eb18f

Browse files
nickbianconickbianco
committed
Add warm start option to Scholz2015GeometryPath
1 parent 96bee31 commit e5eb18f

7 files changed

Lines changed: 187 additions & 5 deletions

File tree

‎Bindings/Java/Matlab/examples/Wrapping/exampleScholz2015GeometryPath.m‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -78,6 +78,11 @@ function exampleScholz2015GeometryPath()
7878
% adding, it defines the insertion of the path.
7979
path.appendPathPoint(model.getBodySet().get('b1'), Vec3(-0.5, 0.1, 0.));
8080

81+
% Enable warm starts in the wrapping solver. At each time step, the
82+
% wrapping solver will use the solution from the previous time step as an
83+
% initial guess for the current time step.
84+
path.setUseWarmStart(true);
85+
8186
% Initialize the system.
8287
state = model.initSystem();
8388
model.updVisualizer().updSimbodyVisualizer().setBackgroundTypeByInt(2);

‎Bindings/Python/examples/Wrapping/exampleScholz2015GeometryPath.py‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -76,6 +76,11 @@
7676
# adding, it defines the insertion of the path.
7777
path.appendPathPoint(model.getBodySet().get('b1'), osim.Vec3(-0.5, 0.1, 0.))
7878

79+
# Enable warm starts in the wrapping solver. At each time step, the
80+
# wrapping solver will use the solution from the previous time step as an
81+
# initial guess for the current time step.
82+
path.setUseWarmStart(True)
83+
7984
# Initialize the system.
8085
state = model.initSystem()
8186
model.updVisualizer().updSimbodyVisualizer().setBackgroundTypeByInt(2)

‎OpenSim/Examples/Wrapping/exampleScholz2015GeometryPath.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,11 @@ int main() {
8282
path.appendPathPoint(model.getComponent<Body>("/bodyset/b1"),
8383
SimTK::Vec3(-0.5, 0.1, 0.));
8484

85+
// Enable warm starts in the wrapping solver. At each time step, the
86+
// wrapping solver will use the solution from the previous time step as an
87+
// initial guess for the current time step.
88+
path.setUseWarmStart(true);
89+
8590
// Initialize the system.
8691
SimTK::State state = model.initSystem();
8792
model.updVisualizer().updSimbodyVisualizer().setBackgroundType(

‎OpenSim/Simulation/Model/Scholz2015GeometryPath.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -189,6 +189,14 @@ int Scholz2015GeometryPath::getNumPathElements() const {
189189
return getProperty_path_elements().size();
190190
}
191191

192+
void Scholz2015GeometryPath::setUseWarmStart(bool useWarmStart) {
193+
set_use_warm_start(useWarmStart);
194+
}
195+
196+
bool Scholz2015GeometryPath::getUseWarmStart() const {
197+
return get_use_warm_start();
198+
}
199+
192200
//=============================================================================
193201
// ABSTRACT PATH INTERFACE
194202
//=============================================================================
@@ -363,6 +371,7 @@ void Scholz2015GeometryPath::extendAddToSystem(
363371
cable.setCurveSegmentAccuracy(1e-10);
364372
cable.setSolverMaxIterations(50);
365373
cable.setAlgorithm(SimTK::CableSpanAlgorithm::Scholz2015);
374+
cable.setUseWarmStart(get_use_warm_start());
366375
_index = cable.getIndex();
367376
}
368377

@@ -419,6 +428,7 @@ void Scholz2015GeometryPath::extendPostScale(const SimTK::State& s,
419428
//=============================================================================
420429
void Scholz2015GeometryPath::constructProperties() {
421430
constructProperty_path_elements();
431+
constructProperty_use_warm_start(false);
422432
}
423433

424434
const Scholz2015GeometryPathObstacle* Scholz2015GeometryPath::getObstacle(

‎OpenSim/Simulation/Model/Scholz2015GeometryPath.h‎

Lines changed: 32 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -162,8 +162,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPathObstacle,
162162
* can touchdown on the obstacle if the surface obstructs the straight line
163163
* segment again.
164164
*
165-
* The path is computed as an optimization problem using the previous optimal
166-
* path as the warm start. This is done by computing natural geodesic
165+
* The path is computed as an optimization problem by computing natural geodesic
167166
* corrections for each curve segment to compute the locally shortest path,
168167
* as described in the following publication:
169168
*
@@ -172,7 +171,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPathObstacle,
172171
* System Dynamics 36, 195–219.
173172
*
174173
* The overall path is locally the shortest, allowing winding over an obstacle
175-
* multiple times, without flipping to the other side.
174+
* multiple times, without flipping to the other side. The wrapping solver can
175+
* optionally use the solution from the previous time step as a warm start to
176+
* improve performance during forward dynamics simulations (see method
177+
* `getUseWarmStart()` for more information).
176178
*
177179
* This class encapsulates `SimTK::CableSpan`, the Simbody implementation of
178180
* this algorithm. For the full details concerning this class, see the Simbody
@@ -425,6 +427,28 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath);
425427
*/
426428
int getNumPathElements() const;
427429

430+
/**
431+
* Set whether the path uses the solution from the previous time step as a
432+
* warm start for the wrapping solver.
433+
*
434+
* Enable this setting when performing forward dyanmics simulations or any
435+
* simulation where path solutions will be computed sequentially in time
436+
* reasonable small time steps. In other scenarios where the configuration
437+
* of the model, and therefore path solutions, may change rapidly, it is
438+
* recommended to disable this setting to avoid inconsistent path solutions.
439+
* If false, the path will always be computed from the curve contact hints
440+
* when realizing to SimTK::Stage::Position. Default: false.
441+
*/
442+
void setUseWarmStart(bool useWarmStart);
443+
444+
/**
445+
* Get whether the path uses the solution from the previous time step as a
446+
* warm start for the wrapping solver.
447+
*
448+
* @see setUseWarmStart()
449+
*/
450+
bool getUseWarmStart() const;
451+
428452
// @}
429453

430454
//** @name `AbstractGeometryPath` interface */
@@ -446,6 +470,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(Scholz2015GeometryPath, AbstractGeometryPath);
446470
// PROPERTIES
447471
OpenSim_DECLARE_LIST_PROPERTY(path_elements, Scholz2015GeometryPathElement,
448472
"The list of elements (path points or obstacles) defining the path.");
473+
OpenSim_DECLARE_PROPERTY(use_warm_start, bool,
474+
"Whether to use the path solution from the previous time step as a "
475+
"warm start for the wrapping solver. If false, the path will always be "
476+
"computed from the curve contact hints when realizing to "
477+
"SimTK::Stage::Position. Default: false.");
449478

450479
// MODEL COMPONENT INTERFACE
451480
void extendConnectToModel(Model& model) override;

‎OpenSim/Tests/Wrapping/testScholz2015GeometryPath.cpp‎

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@
2323

2424
#include <OpenSim/Actuators/ModelFactory.h>
2525
#include <OpenSim/Actuators/Millard2012EquilibriumMuscle.h>
26+
#include <OpenSim/Common/CommonUtilities.h>
2627
#include <OpenSim/Common/Constant.h>
2728
#include <OpenSim/Simulation/Control/PrescribedController.h>
2829
#include <OpenSim/Simulation/Manager/Manager.h>
@@ -31,6 +32,8 @@
3132
#include <OpenSim/Simulation/Model/Scholz2015GeometryPath.h>
3233
#include <OpenSim/Simulation/SimbodyEngine/SliderJoint.h>
3334
#include <OpenSim/Simulation/SimbodyEngine/FreeJoint.h>
35+
#include <OpenSim/Simulation/SimbodyEngine/PinJoint.h>
36+
#include <OpenSim/Simulation/SimbodyEngine/WeldJoint.h>
3437

3538
#include <OpenSim/Simulation/VisualizerUtilities.h>
3639

@@ -146,6 +149,7 @@ TEST_CASE("Suspended pendulum") {
146149

147150
// Set the path's origin and insertion.
148151
Scholz2015GeometryPath& path = actu->updPath<Scholz2015GeometryPath>();
152+
path.setUseWarmStart(true);
149153
path.appendPathPoint(model.getGround(), SimTK::Vec3(-0.1, 0, 0));
150154

151155
// Check that pendulum is at rest with the expected length.
@@ -596,6 +600,7 @@ TEST_CASE("Path with all surfaces and a via point") {
596600

597601
// Create the path object.
598602
Scholz2015GeometryPath* path = new Scholz2015GeometryPath();
603+
path->setUseWarmStart(true);
599604
path->appendPathPoint(*originBody, SimTK::Vec3(0.));
600605

601606
// Add the first torus obstacle.
@@ -1014,3 +1019,126 @@ TEST_CASE("Scaling updates muscle properties") {
10141019
CHECK(scaledMuscle.get_optimal_fiber_length() == 0.123);
10151020
CHECK(scaledMuscle.get_tendon_slack_length() == 0.123);
10161021
}
1022+
1023+
TEST_CASE("Disabling warm starts unifies moment arm calculations") {
1024+
using DataPoints = std::vector<std::pair<double, double>>;
1025+
1026+
// A simple pendulum model with a path wrapping over an ellipsoid obstacle.
1027+
// The model is designed so that, depending on the choice of the default
1028+
// value for the 'angle' coordinate, moment arm calculations may differ when
1029+
// using the warm start feature of Scholz2015GeometryPath.
1030+
auto buildModel = [](bool warmStart, double defaultAngle) {
1031+
Model model;
1032+
model.setName("pendulum");
1033+
1034+
// Define the pendulum bodies: base and head.
1035+
auto* base = new Body("pendulum_base", 1.0, SimTK::Vec3(0),
1036+
SimTK::Inertia(1));
1037+
model.addBody(base);
1038+
auto* head = new Body("pendulum_head", 1.0, SimTK::Vec3(0),
1039+
SimTK::Inertia(1));
1040+
model.addBody(head);
1041+
1042+
// Base welded 1.5 m above ground.
1043+
auto* weld = new WeldJoint("ground_base",
1044+
model.getGround(), SimTK::Vec3(0, 1.5, 0), SimTK::Vec3(0),
1045+
*base, SimTK::Vec3(0), SimTK::Vec3(0));
1046+
model.addJoint(weld);
1047+
1048+
// Add a PinJoint between the base and head driven by the coordinate
1049+
// 'angle'.
1050+
auto* pin = new PinJoint("base_to_head",
1051+
*base, SimTK::Vec3(0), SimTK::Vec3(0),
1052+
*head, SimTK::Vec3(0, 1, 0), SimTK::Vec3(0));
1053+
auto& angle = pin->updCoordinate();
1054+
angle.setName("angle");
1055+
angle.setRangeMin(-0.5);
1056+
angle.setRangeMax(0.5);
1057+
angle.setDefaultValue(defaultAngle);
1058+
model.addJoint(pin);
1059+
1060+
// Attach a contact ellipsoid to the base which the muscle path will
1061+
// wrap over.
1062+
auto* ellipsoid = new ContactEllipsoid(
1063+
SimTK::Vec3(0.1, 0.1, 0.5),
1064+
SimTK::Vec3(-0.01, -0.25, -0.035),
1065+
SimTK::Vec3(0.25, -0.29, 0.075),
1066+
*base);
1067+
ellipsoid->setName("ellipsoid");
1068+
model.addComponent(ellipsoid);
1069+
1070+
// Add muscle with a path between the head and base, wrapping over the
1071+
// ellipsoid.
1072+
auto* muscle = new Millard2012EquilibriumMuscle();
1073+
muscle->setName("muscle");
1074+
muscle->set_path(Scholz2015GeometryPath());
1075+
model.addForce(muscle);
1076+
auto& path = muscle->updPath<Scholz2015GeometryPath>();
1077+
path.appendPathPoint(*head, SimTK::Vec3(0));
1078+
path.appendObstacle(*ellipsoid, SimTK::Vec3(0.10, 0, 0));
1079+
path.appendPathPoint(*base, SimTK::Vec3(0));
1080+
path.setUseWarmStart(warmStart);
1081+
return model;
1082+
};
1083+
1084+
// Sweep over the range of motion of the 'angle' coordinate, computing the
1085+
// moment arm of the muscle path at each point. This replicates the
1086+
// behavior of OpenSim Creator's moment arm plotting functionality, where
1087+
// the underlying bug was originally discovered.
1088+
auto computeDataPoints = [&](bool warmStart, double defaultAngle)
1089+
-> DataPoints {
1090+
Model model = buildModel(warmStart, defaultAngle);
1091+
SimTK::State state = model.initSystem();
1092+
const Coordinate& coordinate =
1093+
model.getComponent<Coordinate>("/jointset/base_to_head/angle");
1094+
const Muscle& muscle = model.getComponent<Muscle>("/forceset/muscle");
1095+
const double start = coordinate.getRangeMin();
1096+
const double end = coordinate.getRangeMax();
1097+
SimTK::Vector x = createVectorLinspace(202, start, end);
1098+
DataPoints data_points;
1099+
for (int i = 0; i < x.size(); ++i) {
1100+
coordinate.setValue(state, x[i]);
1101+
model.equilibrateMuscles(state);
1102+
model.realizeReport(state);
1103+
const double y = muscle.getPath().computeMomentArm(state,
1104+
coordinate);
1105+
data_points.emplace_back(x[i], y);
1106+
}
1107+
return data_points;
1108+
};
1109+
1110+
// Compare two sets of data points sampled at the same coordinate values,
1111+
// checking that the moment arm agrees at every point across the range of
1112+
// motion.
1113+
auto compareDataPoints = [](const DataPoints& lhs, const DataPoints& rhs,
1114+
double tolerance, bool useWarmStart = false) {
1115+
REQUIRE(lhs.size() == rhs.size());
1116+
SimTK::Vector error(static_cast<int>(lhs.size()), 0.0);
1117+
for (int i = 0; i < static_cast<int>(lhs.size()); ++i) {
1118+
error[i] = std::abs(lhs[i].second - rhs[i].second);
1119+
}
1120+
1121+
// If using a warm start, we expect the error to be non-zero (i.e., the
1122+
// moment arm curves different) when using different default values for
1123+
// 'angle'.
1124+
if (useWarmStart) {
1125+
CHECK_THAT(error.norm(),
1126+
!Catch::Matchers::WithinAbs(0.0, tolerance));
1127+
} else {
1128+
CHECK_THAT(error.norm(),
1129+
Catch::Matchers::WithinAbs(0.0, tolerance));
1130+
}
1131+
1132+
CAPTURE(error);
1133+
};
1134+
1135+
// Test that the moment arm curves are identical when using different
1136+
// default values for the 'angle' coordinate, when warm starts are disabled.
1137+
// Conversely, when warm starts are enabled, the moment arm curves should
1138+
// differ depending on the default value of 'angle'.
1139+
bool useWarmStart = GENERATE(false, true);
1140+
CAPTURE(useWarmStart);
1141+
auto lhs = computeDataPoints(useWarmStart, 0.0);
1142+
auto rhs = computeDataPoints(useWarmStart, 0.45);
1143+
compareDataPoints(lhs, rhs, 1e-6, useWarmStart);
1144+
}

‎dependencies/CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -184,8 +184,8 @@ AddDependency(NAME ezc3d
184184

185185
AddDependency(NAME simbody
186186
DEFAULT ON
187-
GIT_URL https://github.com/simbody/simbody.git
188-
GIT_TAG 80a3f10a6daa26c8aaf18f2c672cf385b227e62c
187+
GIT_URL https://github.com/nickbianco/simbody.git
188+
GIT_TAG cable_span_warm_start
189189
CMAKE_ARGS -DBUILD_EXAMPLES:BOOL=OFF
190190
-DBUILD_TESTING:BOOL=OFF
191191
${SIMBODY_EXTRA_CMAKE_ARGS})

0 commit comments

Comments
 (0)