[Moco] Add support for the Bordalba et al. (2023) projection method for kinematic constraints

[nickbianco]

Fixes issue #3528

(Not so) brief summary of changes

Overview

Implemented the "projection" method for enforcing kinematic constraint from the Bordalba et al. (2023) publication. This approach adds two additional sets of variables: 1) a set of auxiliary state variables (that we'll call "projection" states here, to not get confused with auxiliary variables for muscles) and 2) a set of slack variables used to compute the state projection. The projection is enforced by the following equation

where x^prime are the projection states and mu are the slack variables. F is the vector of equations representing the holonomic and non-holonomic kinematic constraints in the system, and F_x is a matrix representing the partial derivative of these equations with respect to the model states. This method works by enforcing the above equation and replacing the regular (non-prime) states in the defect constraints with the projection states.

Computing the projection vector

Rather than try to reimplement exactly how the original publication computes F_x, I instead borrow an idea from Simbody based on how it performs coordinate projection when assembling models with kinematic constraints. For a given state variable, we only need a matrix with a column space normal to the constraint manifold in the system in order to project that state back onto the constraint manifold. Simbody accomplishes by using the constraint matrix G = [P; V; A], which contains acceleration-level errors for all kinematic constraints in the system. Here are the expressions used by Simbody to perform coordinate projection (taken from the Simbody theory manual):

The matrices P and V are the acceleration-level errors for the system's holonomic and non-holonomic constraints, respectively. The matrices W and T are constant, diagonal weighting matrices used to scale the size of the coordinate projection and equations at the top represent the projection algorithm that Simbody uses. The weighting matrices shouldn't be necessary for our implementation, since the mu variables can scale the size of the projection during the optimization. And, clearly, the equations here are replaced by the projection equation from Bordalba et al. (2023), which are enforced as a path constraints in the problem.

Implementation

The implementation is fairly straight-forward, but touches most of the files related to MocoCasADiSolver. The main pieces are as follows:

• MocoCasOCProblem::calcStateProjection() contains the code for computing the state projection term.
• CasOCFunction::StateProjection turns this calculation into a function that CasADi understands.
• CasOCTranscription uses this function to compute path constraint errors to enforce the projection equation above.

Other important changes include:

• New constraints to linearly interpolate Lagrange multiplier values across the mesh interval, since the projection method only needs multipliers at the collocation points.
• States passed to transcription scheme classes are now passes as a VectorMX, where each element of the vector is a matrix (i.e, a CasADi MX) containing the states needed for the mesh interval associated with that index. This makes it easier to implement the projection method, since the last time point of the mesh interval needs to be the projection state, while all other time points use the regular states.
• Various changes to make sure that the projection states and slack variables mu are the correct size when creating or reading in initial guesses.
• Various options added to MocoCasADiSolver to support the new method.

Testing I've completed

• Added double pendulum tests to testMocoConstraints.cpp.
• Added tests for checking bad configurations to testMocoConstraints.cpp.
• Performance benchmarking: [perf-win]

Looking for feedback on...

Any feedback is welcome, but in particular, a care review of the mathematical details of the my implementation (i.e., using Simbody's constraint matrices) would be helpful.

CHANGELOG.md (choose one)

• updated.

[perf-win]

Performance analysis

Platform: Windows, self-hosted runner

Test Name	lhs [secs]	stderr [secs]	rhs [secs]	stderr [secs]	Speedup
Arm26	0.36	0.00	0.34	0.00	1.06
ellipsoid_wrap	4.19	0.00	4.18	0.01	1.00
ellipsoid_wrap_function_based_paths	3.46	0.00	3.48	0.01	0.99
Gait2354	0.41	0.00	0.45	0.00	0.93
MocoSlidingMass	1.48	0.01	1.48	0.00	1.00
MocoSquatToStand	11.21	0.04	11.70	0.09	0.96
passive_dynamic	5.62	0.01	6.04	0.45	0.93
passive_dynamic_noanalysis	3.42	0.00	3.80	0.44	0.90
RajagopalModel	8.95	0.01	9.02	0.01	0.99
ToyDropLanding	13.60	0.01	13.55	0.02	1.00
ToyDropLanding_fbp_stepwisereg	12.48	0.02	12.51	0.02	1.00
ToyDropLanding_function_based_paths	12.59	0.03	12.57	0.04	1.00
ToyDropLanding_nomuscles	0.57	0.00	0.57	0.00	1.00

---

This change is 

[chrisdembia]

How are we able to support kinematic constraints on the main branch without this PR for the Legendre-Gauss etc transcription?

New constraints to linearly interpolate Lagrange multiplier values across the mesh interval, since the projection method only needs multipliers at the collocation points.

I think the Bordalba paper provides an independent Lagrange multiplier at each collocation point; what's the reason for not doing that?

Constraint Jacobian

I'm still trying to understand how similar Simbody's G is to Bordalba's Fx. I think G contains only the second derivative of holonomic constraints, and Fx contains the first and second derivatives, and I haven't yet worked out what this means for the units of G^T * mu (if this can be added to , so I'm thinking we'd have the wrong units if we added G^T * mu to x = (q, u). The Simbody docs https://simbody.github.io/3.7.0/classSimTK_1_1SimbodyMatterSubsystem.html#aa1b31f1a35d519d303c64ad82428f9ae seem to refer to P as both the holonomic constraint Jacobian (first derivative; see multiplyByPqTranspose) and as the acceleration-level position constraints (G = [P;V;A]; see multiplyByGTranspose), so I'm confused by the Simbody docs

Your use of mulitpyByPqTranspose seems correct and in line with Bordalba.

This change warrants updating the Moco Theory Guide, which could also aid with the review.

Reviewed 23 of 29 files at r1, 3 of 3 files at r2, all commit messages.
Reviewable status: 26 of 29 files reviewed, 13 unresolved discussions (waiting on @nickbianco)

---

OpenSim/Moco/MocoDirectCollocationSolver.h line 160 at r2 (raw file):

    OpenSim_DECLARE_PROPERTY(kinematic_constraint_method, std::string,
            "The method used to enforce kinematic constraints in the direct "
            "collocation problem. 'PKT' (default) or 'projection' (only valid "

nit I think most users won't look up what PKT means (actually, I don't see a non-TLA expansion of it in the Bordalba paper) and so they'll remain be confused. Consider Posa2016; it's not much better in terms of understanding for novices but it's somewhat consistent with how we name muscle models at least.

Maybe modified-hermite-simpson?

---

OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h line 51 at r2 (raw file):

/// -------------------------------------------
/// Kinematic constraint and path constraint errors are enforced only at the
/// mesh points. Errors at collocation points within the mesh interval midpoint

Is this really true? If you're using the Bordalba paper, eq 22c, which is evaluated at collocation points, includes the derivatives of kinematic constraints.

Also "midpoint" is probably relevant only for Hermite-Simpson

---

OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp line 66 at r2 (raw file):

        const auto h = m_times(igrid + m_degree + 1) - m_times(igrid);
        const auto x_i = x[imesh](Slice(), Slice(0, m_degree + 1));
        const auto xdot_i = xdot(Slice(),

nit: would it be more consistent to now also package xdot as an MXVector?

---

OpenSim/Moco/MocoCasADiSolver/CasOCProblem.h line 396 at r2 (raw file):

                    OPENSIM_THROW_IF(leafpos == std::string::npos,
                            OpenSim::Exception, "Internal error.");
                    name.replace(leafpos, 6, "_projection/value");

FYI It seems unlikely but this naming scheme could cause confusion with a coordinate named /foo/bar_projection.

---

OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h line 143 at r2 (raw file):

    ///       Legendre-Gauss-Radau collocation, this applies to all time points
    ///       in the interior of the mesh interval.
    void setInterpolateMultiplierMidpoints(bool tf) {

Are you sure that having this as an option is valuable? Would this be to reduce the number of variables?

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h line 176 at r2 (raw file):

    VariablesDM m_shift;
    VariablesDM m_scale;
    casadi::MXVector m_defectStates;

what is this

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 171 at r2 (raw file):

    // containing the states needed to construct the defect constraints for an
    // individual mesh interval.
    m_defectStates = MXVector(m_numMeshIntervals);

just another idea! feel free to ignore

Suggestion:

 m_statesByMeshInterval

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 172 at r2 (raw file):

    // individual mesh interval.
    m_defectStates = MXVector(m_numMeshIntervals);
    m_projectionStateDistances = MX(m_numProjectionStates, m_numMeshIntervals);

move this after the for-loop to better group the m_defectStates code

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 363 at r2 (raw file):

            // the projection states.
            m_defectStates[imesh](Slice(0, m_numProjectionStates), numPts-1) =
                m_unscaledVars[projection_states](Slice(), imesh);

I'm confused by this. Shouldn't the states for the dynamics always come from states, not projection_states (x')? I guess this depends on where the constraint for Bordalba eq (34) is computed in our code.

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 740 at r2 (raw file):

    // Minimize state projection distance.
    if (minimizeStateProjection && m_numProjectionStates) {

What's the reason to not check isKinematicConstraintMethodProjection() here?

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h line 187 at r2 (raw file):

            "'minimize_state_projection_distance' is enabled. "
            "Default: 1e-6.");
    OpenSim_DECLARE_PROPERTY(projection_variable_bounds, MocoBounds,

nit: projection_slack_variable_bounds

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 124 at r2 (raw file):

    // Slack variables.
    // ----------------
    // Check that the guess has the expected slack names from the CasOCProblem.

are you saying "guess" here because we only ever invoke this function for converting a Moco guess to a CasOC guess?

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 132 at r2 (raw file):

                        mocoIt.getSlackNames().end(), expectedName) ==
                    mocoIt.getSlackNames().end()) {
                matchedExpectedSlackNames = false;

In the situation where there are slacks in the Moco guess but they don't match what we expect, are you sure we want to proceed anyway?

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 433 at r2 (raw file):

                getNumNonHolonomicConstraintEquations() +
                getNumAccelerationConstraintEquations(),
                slacks.ptr() + getNumHolonomicConstraintEquations(), true);

What's the length of slacks? Based on the comments I'm guessing it's num_holonomic + num_non_holonomic, but because you're not allocating memory when you create mu_v (bc of the "true" arg), then in the if-statement below, you're writing beyond the memory you're supposed to.

What if we just enforce that getNumAccelerationConstraintEquations() must be 0 (do we support them??), and then len(slacks) == len(mu_v)?

---

OpenSim/Moco/Test/testMocoConstraints.cpp line 998 at r2 (raw file):

                        "Expected the 'hermite-simpson' transcription scheme"));
    }
}

I'm curious how long this test file takes to run now; at some point we may want to split it up.

[chrisdembia]

I'm realizing that the following text from the Moco Theory Guide uses a definition of G that differs from Simbody's G=[P,V,A]:

Reviewable status: 26 of 29 files reviewed, 13 unresolved discussions (waiting on @nickbianco)

[nickbianco]

How are we able to support kinematic constraints on the main branch without this PR for the Legendre-Gauss etc transcription?

We don't, kinematic constraints are still only supported for Hermite-Simpson.

I think the Bordalba paper provides an independent Lagrange multiplier at each collocation point; what's the reason for not doing that?

I've changed the implementation to not require the multiplier interpolation constraints anymore. Instead, I enforce the acceleration-level constraints at all collocation points when using the Bordalba et al. 2023 method. The Posa et al. 2016 method remains the same (no acceleration constraints at collocation points not at mesh points).

Answered some code comments, working my way through the rest.

Reviewable status: 7 of 31 files reviewed, 13 unresolved discussions (waiting on @chrisdembia)

---

OpenSim/Moco/MocoDirectCollocationSolver.h line 160 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

nit I think most users won't look up what PKT means (actually, I don't see a non-TLA expansion of it in the Bordalba paper) and so they'll remain be confused. Consider Posa2016; it's not much better in terms of understanding for novices but it's somewhat consistent with how we name muscle models at least.

Maybe modified-hermite-simpson?

Perhaps Posa2016 and Bordalba2023 then? With good docs I'm okay with using the author names.

---

OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.h line 51 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Is this really true? If you're using the Bordalba paper, eq 22c, which is evaluated at collocation points, includes the derivatives of kinematic constraints.

Also "midpoint" is probably relevant only for Hermite-Simpson

It turns out that we do not enforce any kinematic constraint errors at

---

OpenSim/Moco/MocoCasADiSolver/CasOCLegendreGauss.cpp line 66 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

nit: would it be more consistent to now also package xdot as an MXVector?

Possibly, but I think MXVector uses a bit more memory since it holds multiple "slots" for the same mesh points. This is needed for the states in the Bordalba method, but not for the state derivatives.

---

OpenSim/Moco/MocoCasADiSolver/CasOCSolver.h line 143 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Are you sure that having this as an option is valuable? Would this be to reduce the number of variables?

No longer necessary.

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h line 176 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

what is this

Added some comments.

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 171 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

just another idea! feel free to ignore

I like the change! Done.

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 172 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

move this after the for-loop to better group the m_defectStates code

Done.

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 363 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I'm confused by this. Shouldn't the states for the dynamics always come from states, not projection_states (x')? I guess this depends on where the constraint for Bordalba eq (34) is computed in our code.

Yes, this line directly corresponds to eq (34) in the Bordalba paper. In the paper its the "end state of the Lagrange interpolation", but I generalized it to the end state of the mesh interval for every transcription scheme.

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 740 at r2 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

What's the reason to not check isKinematicConstraintMethodProjection() here?

m_numProjectionStates will be zero if that method returns false.

[nickbianco]

Thanks for the careful review of the docs! Definitely clearer in many places now.

Reviewable status: 31 of 36 files reviewed, 19 unresolved discussions (waiting on @chrisdembia)

---

doc/Moco/MocoTheoryGuide.dox line 521 at r9 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Can you use a single letter for PV ? Or subscripts, like G_pv? Otherwise it's too ambiguous with P * V.

Done.

---

doc/Moco/MocoTheoryGuide.dox line 391 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

The subscript k makes sense to me for l but not for c, for me. Do you mean c_i, which has length d?

Yes, good catch! Fixed.

---

doc/Moco/MocoTheoryGuide.dox line 464 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

specify which 'equations above'

Done.

---

doc/Moco/MocoTheoryGuide.dox line 468 at r10 (raw file):

If F has units of foo, h_F has units of [time_foo] and D*eta has units of [foo/time], correct?

Wouldn't h*F and D*eta have the same units?

Also, can you explain the second row more?

Yes, eta_i is overloaded here. I have made this more clear using the subscript suggestions.

---

doc/Moco/MocoTheoryGuide.dox line 477 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I think eta_i might actually be a matrix (for the other transcriptions above, I think eta could be the whole q vector, not just one of the q's).

Done.

---

doc/Moco/MocoTheoryGuide.dox line 492 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

why does k start at 1 instead of 0?

Because they are only applied at the collocation points. I mistakenly included t_{i,0} in the collocation points above, I've corrected that.

---

doc/Moco/MocoTheoryGuide.dox line 539 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I'm wondering if the bounds for q' and u' need to be slightly larger than those for q and u, to allow the projection to be in either direction, as necessary. But if there's no issue, then let's not worry about it.

This is a good thought. I think in practice it probably doesn't matter too much, especially considering that hard bounds are approximated using barrier functions in IPOPT anyway.

---

doc/Moco/MocoTheoryGuide.dox line 559 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I don't see the primed variables under the variable list in "with respect t"

Done.

---

doc/Moco/MocoTheoryGuide.dox line 565 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Should the Hermite-Simpson section use P instead of G now?

Done.

---

doc/Moco/MocoTheoryGuide.dox line 567 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

The lengedre_gauss(q, u) line assumes N = I, so either remove N here or add a comment saying that Moco requires N = I (and raises an error if it's not...right?).

Done.

---

doc/Moco/MocoTheoryGuide.dox line 569 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I don't see mu under "with respect to"

Done.

---

doc/Moco/MocoTheoryGuide.dox line 586 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

If the eta prime doesn't appear in the defect constraints, then what is the projection doing?

I've added equations to describe where the projection variables appear.

---

OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h line 179 at r10 (raw file):

    // contains the states needed to calculate the defect constraints for a
    // single mesh interval. The Bordalba et al. (2023) kinematic constraint
    // method requires that point the end of one mesh interval and the start of

Done.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h line 194 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Can we make the defaults substantially tighter?

Let's try it!

[nickbianco]

There's one detail I realize now that I never followed up on: for schemes where the endpoint of the mesh interval is also a collocation point (i.e., Legendre-Gauss-Radau), should we be using the auxiliary multibody state variables to evaluate the state derivatives? Currently I am not, but I think there's an argument do this for consistency so that the primary multibody states only appear in the projection equation and not as input arguments for the state derivative functions.

I left a TODO in the docs for this. The Bordalba paper only discusses the method in the context of the Legendre-Gauss scheme, so this is not covered in their methodology.

[chrisdembia]

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

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h line 194 at r10 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Let's try it!

you're missing some -'s lol

[nickbianco]

Reviewable status: 31 of 36 files reviewed, 8 unresolved discussions (waiting on @chrisdembia)

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h line 194 at r10 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

you're missing some -'s lol

Welp lol. Fixed.

[nickbianco]

@chrisdembia Well, it turns out the slack variable bounds I started with were as tight as I could go, at least for the current implementation.

[nickbianco]

@chrisdembia, this is ready for review again.

There is one thing left that potentially warrants discussion: at what points should we enforce acceleration-level kinematic constraint derivatives (and multibody residuals, in implicit mode)? In Bordalba et al. (2023), they only enforce these constraints at collocation points, but in my current implementation I enforce them at all grid points. For example, in Legendre-Gauss collocation, the final point of the mesh interval is not a collocation point, but I enforce the constraints at these points anyway since we define Lagrange multipliers at all grid points. I don't think it makes much of a difference whether or not the constraints are enforced at these points, and it is a bit easier for the implementation to have Lagrange multipliers defined at every grid point.

I discussed this with Antoine and Nicos in the lab, and they both also enforce these constraints at all grid points. I tried to leave some helpful comments in CasOCTranscription.cpp regarding the constraints at these points for the projection method. But happy to discuss the details in person if that's helpful.

[nickbianco]

Okay, one more bonus commit: I realized that the way I was flattening and expanding constraints was a bit inconsistent between the Bordalba and Posa methods. Also, I fixed the problem sparsity when using path constraints (something I previously "broke" and just now noticed). Overall, I re-organized flattening and expanding constraints to make things clearer and updated the comments describing the problem sparsity structure.

(Side note: these updates will be helpful when we add support for the FATROP solver, which expects path and defect constraints to be ordered in a specific way).

[nickbianco]

Based on the MocoSquatToStand perf tests, Moco is running a tad slower with the current changes. After repeated runs of the perf suite, it has been between 3% to 5% slower; this isn't too bad and possibly could be within noise, but it has been consistent.

I've tried reverting the "per mesh interval" state and state derivatives MXVectors back to flat MX inputs to calcDefects(), but this didn't change performance. I noticed that I'm always constructing CasADi functions that are only used for the Bordalba method, I'll try conditionally constructing those and see if that reduces overhead somehow.

If that doesn't do it, then my only other guesses as to what is causing the slowdown are 1) changes to the sparsity of the MocoCasOCProblem functions after breaking up the kinematic constraint errors, 2) I've added somehow added extra function evaluations based on changes in CasOCTranscription, 3) more memory overhead with the added data structures, or 4) it really is just noise and I'm chasing my tail.