[Moco] Add support for the Bordalba et al. (2023) projection method for kinematic constraints #3587
Conversation
…ng kinematic constraints
…om TestMocoSlidingMass.java failures
There was a problem hiding this comment.
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_statesByMeshIntervalOpenSim/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.
There was a problem hiding this comment.
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)
…ion-level constraints in Bordalba2023 // remove multiplier interpolation
There was a problem hiding this comment.
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
xdotas anMXVector?
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_defectStatescode
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, notprojection_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.
There was a problem hiding this comment.
Reviewed 2 of 6 files at r14, 9 of 15 files at r15, 6 of 12 files at r16, 1 of 3 files at r18, 1 of 4 files at r19, 11 of 11 files at r20.
Reviewable status: all files reviewed (commit messages unreviewed), 17 unresolved discussions (waiting on @nickbianco)
CHANGELOG_MOCO.md line 51 at r20 (raw file):
- 2024-04-01: Added `MocoGeneralizedForceTrackingGoal` to enable joint moment tracking in `MocoProblem`s, and added the utility `calcGeneralizeForces()` to
Suggestion:
calcGeneralizedForcesdoc/Moco/MocoStudy.dox line 245 at r6 (raw file):
Previously, nickbianco (Nick Bianco) wrote…
Based on our discussion, unless we change our problem formulation to allow solving the acceleration-level constraints within the ODE (i.e., by not using the model with diabled constraints), then enforcing them with path constraints seems to be the best option. Even if we do eventually want this option, I'd prefer to keep that for future work since this PR is already too large.
FYI, the Simbody 2011 paper uses the term "differential equations on a manifold" (DEM) to describe how kinematic constraints are handled automatically when integrating the system's equations of motion. DEM's are easier to deal with compared to DAE's and allow using typical ODE integrator methods.
I think I still want to talk about this, or I forgot some of what we had discussed, or I forgot everything about Moco. Does f_{\dot{u}} contain the acceleration-level constraints? If so, are we enforcing them twice? And if not, does that mean the Bordalba implementation uses the disabled-constraints model to evaluate f_{\dot{u}} (we should make this clearer in MocoTheoryGuide.dox`)?
doc/Moco/MocoTheoryGuide.dox line 473 at r20 (raw file):
\f[ \begin{alignat*}{1} \textrm{legendre_gauss}_i(\eta_i, F(\eta_i, p)) = \begin{bmatrix} h_i * F(\eta_{i,1:d}, p) - D_i \cdot \eta_{i,0:d} \\ \eta_{i, d+1} - C_i \cdot \eta_{i, 0:d} \end{bmatrix}
F is sometimes a function of more than just \eta. It would be nice to convey this somehow, but I'm not sure how. Something like F(t_i, \cdot_i, \eta_i, p)?
doc/Moco/MocoTheoryGuide.dox line 516 at r20 (raw file):
\mbox{minimize} \quad & \sum_j w_j J_{j}(t_0, t_f, y_0, y_n, x_{0}, x_{n}, \lambda_0, \lambda_n, p, S_{c,j}) + w_{\lambda} \sum_{i=1}^{n} \textrm{quadrature}_i(\|\lambda\|_2^2) \\
nit: Given that we include the penalty on Lagrange multipliers, should we include the penalty on the projection?
doc/Moco/MocoTheoryGuide.dox line 520 at r20 (raw file):
\mbox{subject to} \quad & 0 = \textrm{legendre_gauss}^{\prime}_i(q, u) && i = 1, \ldots, n \\ & 0 = \textrm{legendre_gauss}^{\prime}_i(u, f_{\dot{u}}(t, y, x, \lambda, p)) && i = 1, \ldots, n \\
I see we already used f_{\dot{u}} for Hermite-Simpson but I don't think we define it. We should probably define it.
doc/Moco/MocoTheoryGuide.dox line 573 at r20 (raw file):
\f] where \f$ \eta^{\prime}_{i+1} \f$ represents the auxiliary state variables used in the
Auxiliary makes me think of muscle states but I think you're referring to projection states here, which is a bit confusing.
Code quote:
auxiliary
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h line 181 at r20 (raw file):
// mesh interval and the start of the next mesh interval have different // decision variables representing the state, even though they share the // same time point.
nit: It's not 100% clear to me if/how the second sentence is related to the first.
Code quote:
// defect constraints for a single mesh interval. The Bordalba et al. (2023)
// kinematic constraint method requires that the point at the end of one
// mesh interval and the start of the next mesh interval have different
// decision variables representing the state, even though they share the
// same time point.OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h line 357 at r20 (raw file):
// '*' indicates additional non-zero entry when path constraint // mesh interior points are enforced. '^' indicates points where // acceleration-level kinematic constraints are enforced when using
nit: Consider, instead, splitting up kinematic_# into its blocks (kinematic_perr, kinematic_uerr, kinematic_udoterr) so the special ^` symbol isn't necessary and the sparsity is even more apparent.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 592 at r20 (raw file):
if (m_numMeshInteriorPoints) { const casadi::Function& implicitMultibodyFunction = m_problem.isKinematicConstraintMethodBordalba2023() ?
nit: Consider adding a comment explaining this condition here. I think it's basically that we want to evaluate eq (7) from Bordalba.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 618 at r20 (raw file):
const auto out = evalOnTrajectory(implicitMultibodyFunction, inputs, m_projectionStateIndices); // This overrides the previous function evaluation assignments for
nit: Can you add a comment saying why you do this overriding? I think you mentioned a reason in person related to minimizing function evaluations.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 671 at r20 (raw file):
out.at(2); if (m_problem.isKinematicConstraintMethodBordalba2023()) { std::cout << "out.at(3).size(): " << out.at(3).size() << std::endl;
reminder to remove
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 697 at r20 (raw file):
// we compute algebraic constraints"). m_constraints.kinematic_udoterr(Slice(), m_projectionStateIndices) = out.at(3)(Slice(nqerr + nuerr, nqerr + nuerr + nudoterr),
nit: maybe overkill but you could have member variables for these slices that you reuse (e.g, m_sliceUDotErrRow). That might potentially make the code less readable...so I leave it up to you!
Code quote:
Slice(nqerr + nuerr, nqerr + nuerr + nudoterr)OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp line 380 at r20 (raw file):
// We do not need to append projection states here since they will be // appended later when the guess is resampled by the solver (if needed). bool appendProjectionStates = false;
nit consider moving this within the else below, where it's used.
OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 171 at r20 (raw file):
// the guess. If not, create a vector of zeros to use as the initial guess // for the slack variables. if (matchedExpectedSlackNames) {
nit: I'm curious if it ever helps to warm start with the slacks from a previous iterate, or if it's best to just set the slacks to 0. How common is it to do "mesh refinement" or solve problems iteratively, nowadays? If you have time, it might be worthwhile to ensure we're not inadvertently slowing down convergence by bringing in slack values that aren't actually helping. I suspect the values are so small that it doesn't make a difference either way.
OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 472 at r20 (raw file):
// Holonomic constraint errors. SimTK::Vector mu_p(
nit: same below for mu_v.
Suggestion:
const SimTK::VectorOpenSim/Moco/Test/testMocoConstraints.cpp line 2251 at r20 (raw file):
std::string scheme = GENERATE(as<std::string>{}, "trapezoidal", "hermite-simpson", "legendre-gauss-3", "legendre-gauss-radau-3");
nit: Are you sure it makes sense to put the order in the name rather than as a separate int property?
OpenSim/Moco/Test/testMocoTrack.cpp line 70 at r20 (raw file):
MocoSolution solution = study.solve(); // solution.write("testMocoTrackGait10dof18musc_solution.sto");
uncomment?
There was a problem hiding this comment.
Reviewed all commit messages.
Reviewable status: all files reviewed, 17 unresolved discussions (waiting on @chrisdembia)
CHANGELOG_MOCO.md line 51 at r20 (raw file):
- 2024-04-01: Added `MocoGeneralizedForceTrackingGoal` to enable joint moment tracking in `MocoProblem`s, and added the utility `calcGeneralizeForces()` to
Done.
doc/Moco/MocoStudy.dox line 245 at r6 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
I think I still want to talk about this, or I forgot some of what we had discussed, or I forgot everything about Moco. Does
f_{\dot{u}}contain the acceleration-level constraints? If so, are we enforcing them twice? And if not, does that mean the Bordalba implementation uses the disabled-constraints model to evaluatef_{\dot{u}}(we should make this clearer in MocoTheoryGuide.dox`)?
In Moco, f_{\dot{u}}does not contain the acceleration-level constraints. It does contain the forces applied by the Lagrange multipliers, and we use the udot computed from this function to evaluate the acceleration-level constraint errors.
In Bordalba et al., the mathematical description solves for udot and Lagrange multipliers simultaneously by enforcing the accelerations-level constraints along with the dynamics (Eq. 8). However, later they describe an implicit dynamics form where they solve for the dynamics residual and the acceleration-level constraints as path constraints (Eq. 57) including udot as an algebraic variable in the DAE. Moco (when using explicit dynamics) essentially follows what is described in Eq. 58, where we use udot computed from the dynamics function to enforce the acceleration-level constraints.
Hopefully that makes sense.
doc/Moco/MocoTheoryGuide.dox line 520 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
I see we already used f_{\dot{u}} for Hermite-Simpson but I don't think we define it. We should probably define it.
We had it defined, but it was called f_{\textrm{mb}}. Fixed.
There was a problem hiding this comment.
Reviewed 1 of 2 files at r21, all commit messages.
Reviewable status: 37 of 38 files reviewed, 13 unresolved discussions (waiting on @chrisdembia)
doc/Moco/MocoTheoryGuide.dox line 516 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: Given that we include the penalty on Lagrange multipliers, should we include the penalty on the projection?
Done.
doc/Moco/MocoTheoryGuide.dox line 573 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
Auxiliary makes me think of muscle states but I think you're referring to projection states here, which is a bit confusing.
Done.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h line 181 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: It's not 100% clear to me if/how the second sentence is related to the first.
I removed the second sentence, it's not really necessary here.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.h line 357 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: Consider, instead, splitting up
kinematic_#into its blocks (kinematic_perr,kinematic_uerr, kinematic_udoterr) so the special^` symbol isn't necessary and the sparsity is even more apparent.
Updated, but note that kinematic_udoterr is only enforced in mesh interior points when using the Bordalba method. I added comments to clarify.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 592 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: Consider adding a comment explaining this condition here. I think it's basically that we want to evaluate eq (7) from Bordalba.
Done.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 671 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
reminder to remove
Done.
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 697 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: maybe overkill but you could have member variables for these slices that you reuse (e.g,
m_sliceUDotErrRow). That might potentially make the code less readable...so I leave it up to you!
I think I might leave as-is. It's not great, but having to look somewhere else for the Slice() operation could be more confusing.
OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp line 380 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit consider moving this within the
elsebelow, where it's used.
Done.
OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 171 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: I'm curious if it ever helps to warm start with the slacks from a previous iterate, or if it's best to just set the slacks to 0. How common is it to do "mesh refinement" or solve problems iteratively, nowadays? If you have time, it might be worthwhile to ensure we're not inadvertently slowing down convergence by bringing in slack values that aren't actually helping. I suspect the values are so small that it doesn't make a difference either way.
It was also wondering this myself. It would certainly make things simpler to always set slacks to zero and avoid interpolating non-adjacent time points. It is still quite common to reuse initial guesses.
OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 472 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: same below for
mu_v.
Done.
OpenSim/Moco/Test/testMocoConstraints.cpp line 2251 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: Are you sure it makes sense to put the order in the name rather than as a separate int property?
Hmm, I hadn't considered it. It would only apply to the new schemes. It would make this generator more complicated.
OpenSim/Moco/Test/testMocoTrack.cpp line 70 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
uncomment?
Done.
There was a problem hiding this comment.
Reviewed 1 of 2 files at r21, 6 of 6 files at r22, all commit messages.
Reviewable status: all files reviewed, 13 unresolved discussions (waiting on @chrisdembia)
doc/Moco/MocoTheoryGuide.dox line 473 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
F is sometimes a function of more than just
\eta. It would be nice to convey this somehow, but I'm not sure how. Something likeF(t_i, \cdot_i, \eta_i, p)?
I think F(t_i, \eta_i, p) covers our bases, clarifying that \eta can be any continuous variable (not just continuous states).
OpenSim/Moco/MocoCasADiSolver/CasOCTranscription.cpp line 618 at r20 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit: Can you add a comment saying why you do this overriding? I think you mentioned a reason in person related to minimizing function evaluations.
Yes, I was trying to come up with the simplest implementation that required minimal evaluations of the dynamics functions needed to compute all state derivatives (both regular and "projection" state derivatives). I updated this comment (and the one for explicit multibody dynamics). Let me know if you'd like more clarification.
|
@chrisdembia thanks for the great comments. I'm still thinking about evaluating the benefit of warm-starting slack variables, and whether or not we should just always set them to zero. It would be useful to clean up some places in the code. Also, I'm feeling more confident that how the projections are currently implemented are sufficient. While we are missing some of the submatrices in Bordalba's |
|
@chrisdembia ready for review/approval. I've made (what I thank are) many improvements to
|
|
Added one more thing: a note to the theory guide describing the missing submatrices of |
There was a problem hiding this comment.
Approved! Very exciting stuff!
Regarding testMocoConstraints, are we losing the ability (or test coverage) to solve a problem with prescribed motion constraints that come from a previous solution?
Reviewed 1 of 2 files at r23, 7 of 7 files at r24, 1 of 1 files at r25.
Reviewable status: all files reviewed (commit messages unreviewed), 1 unresolved discussion (waiting on @nickbianco)
doc/Moco/MocoTheoryGuide.dox line 579 at r25 (raw file):
testing and support in Moco. All acceleration-level constraints (including the derivatives of holonomic and non-holonomic constraints) are enforced at all collocation points, not just the mesh points. The cost term preceeded by \f$ w_{proj} \f$ is used to penalize the
nit
Suggestion:
preceded
OpenSim/Moco/MocoDirectCollocationSolver.cpp line 126 at r25 (raw file):
log_warn("of the solution. Consider refining the mesh or adjusting "); log_warn("the constraint tolerance in the MocoProblem."); log_warn(dashes);
beautiful!
There was a problem hiding this comment.
Reviewable status: all files reviewed (commit messages unreviewed), 1 unresolved discussion (waiting on @nickbianco)
There was a problem hiding this comment.
Regarding
testMocoConstraints, are we losing the ability (or test coverage) to solve a problem with prescribed motion constraints that come from a previous solution?
We're not losing the ability to solve a problem using prescribed motion based on a previous solution's kinematics , but yes I did remove the coverage from the test. The spline fit to the kinematic data from the previous solution in the original test was causing small inconsistencies between the coordinate values and speeds, making the tests fail without either 1) larger constraint tolerances or 2) larger slack variables. These weren't huge adjustments, but enough to bother me and influence what I thought the default slack variable sizes should be.
I made the decision to change the test assuming that it was more important to test that prescribed motion constraints work as expected with perfect kinematics. But I suppose that users might run into the issue I had when using GCVSpline with previous solutions or IK data or whatever; I will keep that in mind going forward.
Reviewed 2 of 2 files at r23, 7 of 7 files at r24, all commit messages.
Reviewable status:complete! all files reviewed, all discussions resolved (waiting on @nickbianco)
doc/Moco/MocoTheoryGuide.dox line 579 at r25 (raw file):
Previously, chrisdembia (Christopher Dembia) wrote…
nit
Done
|
Merging! Thanks you @chrisdembia for diving back into Moco :) |
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^primeare the projection states andmuare the slack variables.Fis the vector of equations representing the holonomic and non-holonomic kinematic constraints in the system, andF_xis 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 matrixG = [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
PandVare the acceleration-level errors for the system's holonomic and non-holonomic constraints, respectively. The matricesWandTare 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 themuvariables 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::StateProjectionturns this calculation into a function that CasADi understands.CasOCTranscriptionuses this function to compute path constraint errors to enforce the projection equation above.Other important changes include:
VectorMX, where each element of the vector is a matrix (i.e, a CasADiMX) 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.muare the correct size when creating or reading in initial guesses.MocoCasADiSolverto support the new method.Testing I've completed
testMocoConstraints.cpp.testMocoConstraints.cpp.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)
[perf-win]
Performance analysis
Platform: Windows, self-hosted runner
This change is