Computational improvements to static optimization

[nathantpickle-cfdrc]

Brief summary of changes

The static optimization code had inefficiencies due to how the state derivatives were calculated. This PR pre-calculates all state derivatives to improve speed. The analysis in analyze1 in testArm26() ran approximately 25% faster with these changes.

Testing I've completed

Ran testStaticOptimization.cpp with no failures.

Looking for feedback on...

Should be ready to merge.

CHANGELOG.md (choose one)

• no need to update because...this is a minor change for improved speed

---

This change is 

[nickbianco]

Nice work @nathantpickle-cfdrc! I left a handful of comments, mostly related to formatting (e.g., tabs to spaces), documentation, and one unused variable.

Reviewed 4 of 4 files at r1, all commit messages.
Reviewable status: all files reviewed, 8 unresolved discussions (waiting on @nathantpickle-cfdrc)

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 176 at r1 (raw file):

*
* @param aStatesDerivativeStore States derivative storage.
*/

These Doxygen-style comments won't actually appear in Doxygen, but I see that you're copying the existing comments. This is a bit out of scope of this PR, but if you wanted to make another improvement, you could move these comments and the others nearby to the header file so Doxygen can parse them.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 623 at r1 (raw file):

        }

		// No longer use a function to calcDerivative, but instead use the _statesDerivativeSplineSet from SO FAST 

Rather than commenting on what we used to do (which developers won't see anymore), just state what is happening now, e.g., "Retrieve state derivatives..."

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 628 at r1 (raw file):

		int nu = _model->getNumSpeeds();
		Array<double> targetStateArray(0.0, nq + nu);
		_statesDerivativeStore->getDataAtTime(t, nq + nu, targetStateArray);

Is _statesDerivativeSplineSet not actually used?

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 629 at r1 (raw file):

		Array<double> targetStateArray(0.0, nq + nu);
		_statesDerivativeStore->getDataAtTime(t, nq + nu, targetStateArray);
		double targetAcceleration = targetStateArray[ind];

Convert tabs to spaces.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 40 at r1 (raw file):

/**
 * This class implements static optimization to compute muscle forces and
 * activations without re-splining during the calculation of state derivatives.

Re-splining what?

---

OpenSim/Analyses/StaticOptimizationTarget.h line 71 at r1 (raw file):

    GCVSplineSet _statesSplineSet;
	const Storage *_statesDerivativeStore;
	GCVSplineSet _statesDerivativeSplineSet;

Remove tabs.

---

OpenSim/Analyses/StaticOptimization.cpp line 324 at r1 (raw file):

    target.setStatesSplineSet(_statesSplineSet);
    target.setStatesDerivativeStore(_statesDerivativeStore);
	target.setStatesDerivativeSplineSet(_statesDerivativeSplineSet);

Remove tab.

---

OpenSim/Analyses/StaticOptimization.cpp line 585 at r1 (raw file):

	// Add spline set for the first derivative of states to pass to target for SO FAST and no longer need re-splining to calcDerivative at each time step
	_statesDerivativeStore = _statesSplineSet.constructStorage(1);
	_statesDerivativeSplineSet = GCVSplineSet(5, _statesDerivativeStore);

Convert tabs to spaces.

(I actually didn't know about the constructStorage() trick for constructing derivatives -- nice!)

[nathantpickle-cfdrc]

Reviewable status: 1 of 4 files reviewed, 8 unresolved discussions (waiting on @nickbianco)

---

OpenSim/Analyses/StaticOptimization.cpp line 324 at r1 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove tab.

This should be fixed throughout the files.

My linter made a fair number of whitespace changes - please let me know if I need to revert those.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 176 at r1 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

These Doxygen-style comments won't actually appear in Doxygen, but I see that you're copying the existing comments. This is a bit out of scope of this PR, but if you wanted to make another improvement, you could move these comments and the others nearby to the header file so Doxygen can parse them.

I moved all the Doxygen-style comments to the header.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 623 at r1 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Rather than commenting on what we used to do (which developers won't see anymore), just state what is happening now, e.g., "Retrieve state derivatives..."

Fixed here as well as a couple of other places I noticed.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 628 at r1 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Is _statesDerivativeSplineSet not actually used?

After taking a second look at this, the _statesDerivativeSplineSet was unused and splines of the derivatives are not necessary. It is sufficient for the algorithm to retrieve the state derivatives that were pre-calculated using the splined states.

[nickbianco]

@nathantpickle-cfdrc thanks for the nice changes! I left another round of comments.

Reviewed 3 of 3 files at r2, all commit messages.
Reviewable status: all files reviewed, 18 unresolved discussions (waiting on @nathantpickle-cfdrc)

---

OpenSim/Analyses/StaticOptimization.cpp line 324 at r1 (raw file):

Previously, nathantpickle-cfdrc wrote…

This should be fixed throughout the files.

My linter made a fair number of whitespace changes - please let me know if I need to revert those.

Since I made some formatting requests already, I went ahead and reviewed the whitespace changes and made a few additional suggestions. However, in general, I would avoid adding a bunch of whitespace changes to an ongoing PR in the future, as it can complicate the review process.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 176 at r1 (raw file):

Previously, nathantpickle-cfdrc wrote…

I moved all the Doxygen-style comments to the header.

Great!

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 314 at r2 (raw file):

        const StaticOptimizationTarget* aTarget, double* dx, const Vector& x,
        Vector& dpdx) {
    if (aTarget == NULL) return (-1);

Remove parenthesis from return value.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 318 at r2 (raw file):

    // CONTROLS
    int nx = aTarget->getNumParameters();
    if (nx <= 0) return (-1);

Remove parenthesis from the return value.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 210 at r2 (raw file):

    /**
     * Compute derivatives of performance with respect to the
     * controls by central differences.  Note that the gradient array should

minor:

Suggestion:

     * controls by central differences. Note that the gradient array should

---

OpenSim/Analyses/StaticOptimizationTarget.h line 229 at r2 (raw file):

    /**
     * Compute performance given parameters.

Suggestion:

     * Compute the objective function given the optimization parameters.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 232 at r2 (raw file):

     *
     * @param parameters Vector of optimization parameters.
     * @param performance Value of the performance criterion.

Suggestion:

     * @param performance Value of the objective function.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 239 at r2 (raw file):

    /**
     * Compute the gradient of performance given parameters.

Suggestion:

     * Compute the gradient of the objective function given the optimization parameters.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 242 at r2 (raw file):

     *
     * @param parameters Vector of optimization parameters.
     * @param gradient Derivatives of performance with respect to the

Suggestion:

     * @param gradient Derivatives of the cost function with respect to the

---

OpenSim/Analyses/StaticOptimizationTarget.h line 250 at r2 (raw file):

    /**
     * Compute acceleration constraints given parameters.

Suggestion:

     * Compute acceleration constraints given the optimization parameters.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 260 at r2 (raw file):

    /**
     * Compute the gradient of constraint given parameters.

Suggestion:

     * Compute the constraint Jacobian given the optimization parameters.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 263 at r2 (raw file):

     *
     * @param parameters Vector of parameters.
     * @param jac Derivative of constraint with respect to the parameters.

Suggestion:

     * @param jac The constraint Jacobian with respect to the optimization parameters.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 271 at r2 (raw file):

private:
    /**
     * Compute all constraints given parameters.

Suggestion:

     * Compute all constraints given the optimization parameters.

---

OpenSim/Analyses/StaticOptimization.cpp line 249 at r2 (raw file):

 */
Storage* StaticOptimization::getActivationStorage() {
    return (_activationStorage);

Remove parentheses around the return value.

---

OpenSim/Analyses/StaticOptimization.cpp line 331 at r2 (raw file):

        // Some IPOPT-specific settings
        optimizer->setLimitedMemoryHistory(
                500); // works well for our small systems

I would revert this line break. You could delete the comment to fit within the column limit.

---

OpenSim/Analyses/StaticOptimization.cpp line 492 at r2 (raw file):

 */
int StaticOptimization::begin(const SimTK::State& s) {
    if (!proceed()) return (0);

Remove parentheses from the return value.

---

OpenSim/Analyses/StaticOptimization.cpp line 634 at r2 (raw file):

    record(s);

    return (0);

Remove parentheses.

---

OpenSim/Analyses/StaticOptimization.cpp line 646 at r2 (raw file):

 */
int StaticOptimization::end(const SimTK::State& s) {
    if (!proceed()) return (0);

Remove parenthesis from return value.

---

OpenSim/Analyses/StaticOptimization.cpp line 650 at r2 (raw file):

    record(s);

    return (0);

Remove parentheses from return value.

[nathantpickle-cfdrc]

Thanks! In addressing some of the feedback I noticed that the Doxygen comments had gotten stale. I went through and updated the parameter names in the documentation to match the actual parameters.

Reviewable status: 1 of 4 files reviewed, 17 unresolved discussions (waiting on @nickbianco)

---

OpenSim/Analyses/StaticOptimization.cpp line 324 at r1 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Since I made some formatting requests already, I went ahead and reviewed the whitespace changes and made a few additional suggestions. However, in general, I would avoid adding a bunch of whitespace changes to an ongoing PR in the future, as it can complicate the review process.

Thanks! I'll be careful of that in the future.

---

OpenSim/Analyses/StaticOptimization.cpp line 249 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove parentheses around the return value.

Done.

---

OpenSim/Analyses/StaticOptimization.cpp line 331 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

I would revert this line break. You could delete the comment to fit within the column limit.

Done.

---

OpenSim/Analyses/StaticOptimization.cpp line 492 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove parentheses from the return value.

Done.

---

OpenSim/Analyses/StaticOptimization.cpp line 634 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove parentheses.

Done.

---

OpenSim/Analyses/StaticOptimization.cpp line 646 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove parenthesis from return value.

Done.

---

OpenSim/Analyses/StaticOptimization.cpp line 650 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove parentheses from return value.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 314 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove parenthesis from return value.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 318 at r2 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Remove parenthesis from the return value.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 229 at r2 (raw file):

    /**
     * Compute performance given parameters.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 232 at r2 (raw file):

     *
     * @param parameters Vector of optimization parameters.
     * @param performance Value of the performance criterion.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 239 at r2 (raw file):

    /**
     * Compute the gradient of performance given parameters.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 242 at r2 (raw file):

     *
     * @param parameters Vector of optimization parameters.
     * @param gradient Derivatives of performance with respect to the

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 250 at r2 (raw file):

    /**
     * Compute acceleration constraints given parameters.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 260 at r2 (raw file):

    /**
     * Compute the gradient of constraint given parameters.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 263 at r2 (raw file):

     *
     * @param parameters Vector of parameters.
     * @param jac Derivative of constraint with respect to the parameters.

Done.

---

OpenSim/Analyses/StaticOptimizationTarget.h line 271 at r2 (raw file):

private:
    /**
     * Compute all constraints given parameters.

Done.

[nickbianco]

Looks great! Thanks for doing some clean up along the way.

I have one final request: let's add a change log entry to CHANGELOG.md. I think the 25% speed improvement is worth highlighting there. After that, we'll be good to merge.

Reviewed 3 of 3 files at r3, all commit messages.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @nathantpickle-cfdrc)

[nickbianco]

@nathantpickle-cfdrc the style CI test failed. Looks like some tabs snuck back into StaticOptimization.h:

./OpenSim/Analyses/StaticOptimization.h:82:	Storage *_statesDerivativeStore;
./OpenSim/Analyses/StaticOptimization.h:83:	GCVSplineSet _statesDerivativeSplineSet;
Tabs found in the lines shown above. See CONTRIBUTING.md about tabs.

[nickbianco]

Thanks @nathantpickle-cfdrc!

Reviewed 2 of 2 files at r4, all commit messages.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on @nathantpickle-cfdrc)

[nickbianco]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nathantpickle-cfdrc)

---

CHANGELOG.md line 92 at r4 (raw file):

  tendon force bounds rather than fiber length bounds. (#4040)
- Fixed bugs in `PolynomialPathFitter` when too few coordinate samples were provided. (#4039)
- Pre-calculate state derivatives in static optimization to reduce computational time by as much as 25% (#4037)

Sorry to add one more comment after giving the thumbs-up, but I think we could add a bit more detail here. Something along the lines of "In StaticOptimization, state derivatives are now pre-calculated at the time points of the original data, rather than calculated during optimization from splines. This change reduces computational time by as much as 25%. (#4037)"

I have one last follow up question based on the that: did you double check that the time points the state derivatives are calculated from are consistent with the time points that StaticOptimization uses for the optimization? Just double checking that users will get consistent results. (The tolerances in testStaticOptimization are a bit loose.)

[nickbianco]

@nathantpickle-cfdrc, actually, I have one last comment to address. Just want to clarify that these changes won't produce unexpected behavior for users.

[nathantpickle-cfdrc]

No problem - let me run a few more checks to make sure.

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nathantpickle-cfdrc)

[nathantpickle-cfdrc]

Reviewable status: 2 of 6 files reviewed, 3 unresolved discussions (waiting on @nickbianco)

---

CHANGELOG.md line 92 at r4 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Sorry to add one more comment after giving the thumbs-up, but I think we could add a bit more detail here. Something along the lines of "In StaticOptimization, state derivatives are now pre-calculated at the time points of the original data, rather than calculated during optimization from splines. This change reduces computational time by as much as 25%. (#4037)"

I have one last follow up question based on the that: did you double check that the time points the state derivatives are calculated from are consistent with the time points that StaticOptimization uses for the optimization? Just double checking that users will get consistent results. (The tolerances in testStaticOptimization are a bit loose.)

@nickbianco - Ryan and I went back and took another look at this. Our understanding is that static optimization iterates through the original timesteps and performs optimization at each timestep without perturbing the time value. If this is correct, it should not affect anything to pre-calculate the state derivatives and store them.

We added a couple of tests that seem to confirm that the changes will not produce inconsistent results. Let me know your thoughts.

---

Applications/Analyze/test/testStaticOptimization.cpp line 268 at r5 (raw file):

    ASSERT_EQUAL(forces.getColumnLabels().findIndex("TRImed"), -1);

    StaticOptimization& statOpt =(StaticOptimization&)model.getAnalysisSet().get("StaticOptimization");

Added a check that the time values of the stored derivatives match the original time values associated with the states.

This check could also be added to a different test if desired.

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 499 at r5 (raw file):

        Array<double> targetStateArray(0.0, nq + nu);
        
        //Ensure state time matches stored time associated with derivative

@nickbianco added this check to confirm that the stored time value matches the time value in the state that is passed in to static opt.

My assumption is that if the optimizer messes with the time in the state, it should trigger this exception. When running testStaticOptimization.cpp this exception was not triggered.

If this satisfactorily answers your question, it can probably be removed from the final PR.

[nickbianco]

Thanks @nathantpickle-cfdrc! I think our bases are covered now. Let's remove the block of code in StaticOptimizationTarget and then we should be good to merge.

Reviewed 4 of 4 files at r5, all commit messages.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nathantpickle-cfdrc)

---

OpenSim/Analyses/StaticOptimizationTarget.cpp line 499 at r5 (raw file):

Previously, nathantpickle-cfdrc wrote…

@nickbianco added this check to confirm that the stored time value matches the time value in the state that is passed in to static opt.

My assumption is that if the optimizer messes with the time in the state, it should trigger this exception. When running testStaticOptimization.cpp this exception was not triggered.

If this satisfactorily answers your question, it can probably be removed from the final PR.

I think your test and the existing tests are satisfactory to address my comment about preserving the time vector. Let's remove this block of code to avoid the overhead of findIndex() within the static optimization target.

[nickbianco]

Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @nathantpickle-cfdrc)

[nathantpickle-cfdrc]

Will do. We also had to create a getStatesDerivativeStore() method in StaticOptimization.h to allow us to retrieve the stored state derivatives for the test we added to testArm26DisabledMuscles(). That method is not likely to be used anywhere besides the test. What do you think about removing both getStatesDerivativeStore() and the test we added?

[nickbianco]

I think we should keep the test, and I think it's fine to keep getStatesDerivativeStore().

I added one quick request for the added test. After addressing that, we should be good to merge.

Reviewable status: all files reviewed, 2 unresolved discussions (waiting on @nathantpickle-cfdrc)

---

Applications/Analyze/test/testStaticOptimization.cpp line 268 at r5 (raw file):

    ASSERT_EQUAL(forces.getColumnLabels().findIndex("TRImed"), -1);

    StaticOptimization& statOpt =(StaticOptimization&)model.getAnalysisSet().get("StaticOptimization");

Add a brief comment describing this this test.

[nathantpickle-cfdrc]

Reviewable status: 4 of 6 files reviewed, 1 unresolved discussion (waiting on @nickbianco)

---

Applications/Analyze/test/testStaticOptimization.cpp line 268 at r5 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Add a brief comment describing this this test.

Done.

[nickbianco]

Thanks @nathantpickle-cfdrc, the code is looking good now.

Sorry to keep moving the finish line on you, but I'm going to ask you for one more sanity check: could you plot the results from the static optimization tests before and after this change? I think it would be helpful to confirm that everything works qualitatively, especially since the regression tests have fairly loose tolerances.

[nathantpickle-cfdrc]

No problem - we don't want to inadvertently break anything! Ryan helped out and we ran a couple more tests.

We used the OpenSim 4.3 GUI to test the original static optimization code, and ran static optimization from the command line using the same setup file to test the modified code.

The first test we ran was a torque-actuated double pendulum. There are two torque actuators, tau0 and tau1. The plots are exactly overlaid, so the blue "original" line is not visible.

We also ran a more complex model to provide a more rigorous stress test of the updated algorithm. For that, we used a 5-second simulation of the Vasavada neck model performing a motion from 30 degrees to 60 degrees of head yaw. We plotted several of the muscle results and reserve actuators and found that they also overlaid each other exactly.

Happy to run any other test cases you'd like to see before completing this merge.

Reviewable status: 4 of 6 files reviewed, 1 unresolved discussion (waiting on @nickbianco)

[nickbianco]

Thanks @nathantpickle-cfdrc! Looks great. I think we've officially covered all the bases.

Merging!