[Moco] Adding support for PrescribedController and future controller types

[nickbianco]

Fixes issue #3605

Brief summary of changes

This PR adds support for models with PrescribedControllers in Moco optimizations, and lays some ground work for future controller types that will map OCP controls to Model controls. Therefore, going forward, the relationship between OCP controls and an OpenSim model controls will no longer necessarily be a one-to-one mapping. These changes include support for the two simplest control mapping cases: 1) one-to-one mapping (i.e., current behavior) and 2) controls associated with a PrescribedController are excluded from the OCP.

Changes include:

• Added ControlAllocator which manages a Vector discrete variable containing the OCP controls and provides Output channels to map these controls to OpenSim Model controls.
• Removed DiscreteController in favor of ControlAllocator.
• Added the abstract class InputController. This owns a list Input which concrete implementations can use to take a subset of the Output values from ControlAllocator to be mapped to Model controls.
• Added ActuatorInputController, the simplest concrete implementation of InputController. This controller provides a one-to-one mapping between OCP controls and Model controls. This enables the original Moco behavior for controls, but also supports handling a subset of Model controls (e.g., when a `PrescribedController is present).
• Added options to ignore controlled Actuators in relevant MocoGoals via setIgnoreControlledActuators().
• Various minor drive-by fixes.

Testing I've completed

Added a test for PrescribedController to testMocoInterface.cpp.

Looking for feedback on...

• The name "inputs" for the InputController's list Input. Something less generic would probably be preferred, but I'm having trouble coming up with a term to describe the scalar values that drive an InputController.
• Anything else.

CHANGELOG.md (choose one)

• updated.

---

This change is 

[nickbianco]

I've updated the InputController interface to better support generic control names (i.e., non-actuator control names). I've also made an update where Model control names not included in the trajectories returned by CasADi and tropter are added to the MocoTrajectory returned to the user for convenience. Sorry to expand a bit, but I think this is a pretty essential addition.

Working now on Doxygen documentation.

[nickbianco]

@chrisdembia ready for review.

Reviewable status: 15 of 39 files reviewed, 9 unresolved discussions (waiting on @chrisdembia)

[chrisdembia]

Reviewed 7 of 23 files at r4, 6 of 14 files at r5.
Reviewable status: 28 of 43 files reviewed, 7 unresolved discussions (waiting on @nickbianco)

---

OpenSim/Moco/MocoProblem.h line 367 at r5 (raw file):

            "the bounds for muscle control (excitation), using "
            "min/max control if explicit control bounds are not "
            "provided. This applies to all controls, including those defined "

"This includes muscles controlled by user-added Controllers"? This setting applies to muscle activations, not controls, so I'm suggesting replacing "applies to all controls" with a muscle-related term

---

OpenSim/Moco/MocoProblemRep.h line 151 at r5 (raw file):

    bool isPrescribedKinematics() const { return m_prescribedKinematics; }
    /// Do we need to compute controls from the model (e.g., because the model
    /// contains user-defined controllers)?

Consider adding something like "If the model does not contain user-defined controls, then we prefer to use the controls directly from the optimal control problem, for efficiency" Otherwise it seems like we should just always compute controls from the model.

[nickbianco]

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

---

OpenSim/Moco/MocoProblem.h line 367 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

"This includes muscles controlled by user-added Controllers"? This setting applies to muscle activations, not controls, so I'm suggesting replacing "applies to all controls" with a muscle-related term

Done.

---

OpenSim/Moco/MocoProblemRep.h line 151 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Consider adding something like "If the model does not contain user-defined controls, then we prefer to use the controls directly from the optimal control problem, for efficiency" Otherwise it seems like we should just always compute controls from the model.

Done.

[nickbianco]

@aymanhab, this is almost ready to merge, did you have anything else you wanted to check/comment on?

[aymanhab]

Thanks for checking @nickbianco I only had a comment about the naming and that has been fully addressed, if the rest in the bowls of Moco has been reviewed by @chrisdembia then LGTM. Thank you

Reviewed 1 of 14 files at r5, 2 of 2 files at r6, all commit messages.
Reviewable status: 29 of 43 files reviewed, 7 unresolved discussions (waiting on @chrisdembia)

[aymanhab]

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

[chrisdembia]

Reviewed 7 of 23 files at r4, 7 of 14 files at r5, all commit messages.
Reviewable status: all files reviewed, 11 unresolved discussions (waiting on @nickbianco)

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp line 395 at r5 (raw file):

    if (getProblemRep().getComputeControlsFromModel()) {
        updateSolutionControls(mocoSolution, casSolution);
        mocoSolution = convertToMocoTrajectory<MocoSolution>(casSolution);

Is there a reason you need to append the model-based controls in the casSolution? Can you directly put them in the MocoSolution?

Also, what's the reason for doing this here instead of a level deeper, so that the casSolution already has the model-based controls? That seems like a better parallel to computing the controls in the MocoCasOCProblem.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp line 473 at r5 (raw file):

    // Append the missing controls to the CasADi solution and
    // regenerate the MocoSolution.

you don't regenerate the MocoSolution here.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 438 at r5 (raw file):

        auto& simtkStateDisabledConstraints =
                mocoProblemRep->updStateDisabledConstraints();
        if (m_computeControlsFromModel) {

Seems like there's potentially an opportunity for a helper function for getting controls that you could use across MocoCasOCProblem.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 441 at r5 (raw file):

            m_endpointIntegrandControls =
                    modelDisabledConstraints.getDefaultControls();
            modelDisabledConstraints.computeControls(

In the previous iteration you did realizeVelocity() and getControls(); what's the reason for the switch here?

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 447 at r5 (raw file):

                    mocoProblemRep->getControlDistributorDisabledConstraints();
            m_endpointIntegrandControls =
                    controlDistributorDisabledConstraints.getControls(

remind me... are we guaranteed that the controls are in the same order regardless of the value of m_computeControlsFromModel? Because the goals's initializeImpl() function often stores indices into the controls.

I guess more importantly, the indices gathered by goals in initializeImpl() must be valid inside the goal functions for computing the goal.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 449 at r5 (raw file):

                    controlDistributorDisabledConstraints.getControls(
                        simtkStateDisabledConstraints);
        }

In both branches, the memory for the controls is already allocated within the SimTK::State, which is already threadsafe due to the use of the jar things. So unless I'm missing something, I don't think you need to copy the state vectors into a thread_local member variable. You could have a helper function that returns a const SimTK::Vector* (or maybe a const SimTK::Vector&) and provide that in the calcIntegrand struct argument.

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 83 at r3 (raw file):

    } else {
        controlIndexMap = model.getComponentList<ControlAllocator>().begin()
                                ->getControlIndexMap();

I expected to see a call to ControlDistributor::getControlIndexMap() somewhere; can you remove it?

Why can we now assume the controls map is the same regardless of computeControlsFromModel?

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 107 at r5 (raw file):

        if (weight != 0.0) {
            std::string msg = "MocoControlGoal: Control '" + controlName +
                    "' has weight " + std::to_string(weight) + ".";

do you use this anywhere?

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 131 at r5 (raw file):

    }

    setRequirements(1, 1, SimTK::Stage::Model);

Oh interesting, so the goal does actually depend on Velocity, but it's hidden from the goal developer. Interesting. I think that's fine; I just hadn't considered that before. I wonder if that's worth documenting in the doxygen comment for setRequirements().

---

OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp line 121 at r6 (raw file):

    setRequirements(
            0, (int)m_indices_states.size() + (int)m_indices_controls.size(),
            SimTK::Stage::Velocity);

Hmm for the MocoControlGoal, you left this at "Model". I think setRequirements() needs docs about what to set the stage to for goals that depend on controls.

---

OpenSim/Moco/Test/testMocoInterface.cpp line 2301 at r5 (raw file):

}

TEST_CASE("MocoControlGoal: ignoring controlled actuators") {

cool test!

---

OpenSim/Moco/tropter/TropterProblem.h line 368 at r5 (raw file):

        this->setSimTKState(in);

        if (m_computeControlsFromModel) {

I think there's an option for a shared helper function that you use in Tropter and CasADi.

Also, maybe we should get rid of Tropter??? Has anyone used it? lol.

[nickbianco]

Reviewable status: all files reviewed, 11 unresolved discussions (waiting on @chrisdembia)

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp line 395 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Is there a reason you need to append the model-based controls in the casSolution? Can you directly put them in the MocoSolution?

Also, what's the reason for doing this here instead of a level deeper, so that the casSolution already has the model-based controls? That seems like a better parallel to computing the controls in the MocoCasOCProblem.

Good suggestion, will do.

I didn't put them a level deeper because the next level down is CasOCSolver, which knows nothing about the model. I suppose I could pass the model control names to CasOCSolver, but I opted to add a helper function to MocoProblemRep which makes things cleaner.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp line 473 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

you don't regenerate the MocoSolution here.

Done.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 438 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Seems like there's potentially an opportunity for a helper function for getting controls that you could use across MocoCasOCProblem.

Done.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 441 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

In the previous iteration you did realizeVelocity() and getControls(); what's the reason for the switch here?

I was trying to avoid the unncecessary realizeVelocity() call when getting controls from the ControlDistributor. I found a better way to do with with helper functions (based on your suggestion above).

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 447 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

remind me... are we guaranteed that the controls are in the same order regardless of the value of m_computeControlsFromModel? Because the goals's initializeImpl() function often stores indices into the controls.

I guess more importantly, the indices gathered by goals in initializeImpl() must be valid inside the goal functions for computing the goal.

On line 167 of MocoProblemRep I added logic to ensure that the ControlDistributor controls are in system order when we need to access them directly:

// If we don't need to compute controls from the model then we need to add
// the controls to the ControlDistributor in system order, which is the
// order expected by MocoGoals. Otherwise, just get the control names from
// the InputControllers in the model based on the input aliases they expect.
if (m_computeControlsFromModel) {
    for (const auto& controller :
            m_model_base.getComponentList<InputController>()) {
        const auto expectedAliases =
                controller.getExpectedInputChannelAliases();
        for (const auto& alias : expectedAliases) {
            controlDistributorUPtr->addControl(alias);
        }
    }
} else {
    // This is valid since we already checked above that the order of the
    // controls in the model matches the system controls.
    for (const auto& controlName :
            createControlNamesFromModel(m_model_base)) {
        controlDistributorUPtr->addControl(controlName);
    }
}

I suppose this logic could somehow be added to ControlDistributor if we wanted to.

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 449 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

In both branches, the memory for the controls is already allocated within the SimTK::State, which is already threadsafe due to the use of the jar things. So unless I'm missing something, I don't think you need to copy the state vectors into a thread_local member variable. You could have a helper function that returns a const SimTK::Vector* (or maybe a const SimTK::Vector&) and provide that in the calcIntegrand struct argument.

Avoided the extra member variables with my new helper functions.

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 83 at r3 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I expected to see a call to ControlDistributor::getControlIndexMap() somewhere; can you remove it?

Why can we now assume the controls map is the same regardless of computeControlsFromModel?

Currently, my design is that the controls vector passed to MocoGoals is always the model controls vector, so there's no need for any index checking from the ControlDistributor. Eventually, we will need a way to design cost functions using the non-model control values (e.g., synergy excitations). I'm not sure if the best way to do this is append the "input" controls to the controls vector passed to all MocoGoals, or create a subset of MocoGoals designed to optimize the input control values (e.g., MocoInputControlGoal). Perhaps we have another suite of goals akin to MocoOutputGoal, (e.g., MocoInputGoal, MocoFinalInputGoal, MocoInputTrackingGoal, etc.). I plan to address this in a subsequent PR.

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 107 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

do you use this anywhere?

Done.

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 131 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Oh interesting, so the goal does actually depend on Velocity, but it's hidden from the goal developer. Interesting. I think that's fine; I just hadn't considered that before. I wonder if that's worth documenting in the doxygen comment for setRequirements().

Will do.

---

OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp line 121 at r6 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Hmm for the MocoControlGoal, you left this at "Model". I think setRequirements() needs docs about what to set the stage to for goals that depend on controls.

I meant to switch this back to Stage::Velocity (done). Added docs.

---

OpenSim/Moco/tropter/TropterProblem.h line 368 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I think there's an option for a shared helper function that you use in Tropter and CasADi.

Also, maybe we should get rid of Tropter??? Has anyone used it? lol.

There is an option for that! Done.

As for Tropter...probably? I doubt many use it, unless they really need to optimize mass or something (but we could make that work with CasADi anyway). It would be nice to remove some dependencies.

[nickbianco]

Reviewable status: 32 of 46 files reviewed, 11 unresolved discussions (waiting on @aymanhab and @chrisdembia)

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 131 at r5 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Will do.

Done.

[chrisdembia]

I have some documentation nits but

Nicely done!

Reviewed 11 of 12 files at r7, all commit messages.
Reviewable status: 43 of 44 files reviewed, 3 unresolved discussions (waiting on @aymanhab and @nickbianco)

---

OpenSim/Moco/MocoProblemRep.h line 338 at r7 (raw file):

    /// ControlDistributor. This function is intended for use by solvers to
    /// compute controls needed by MocoGoal%s and MocoPathConstraint%s.
    const SimTK::Vector& getControls(const SimTK::State& state) const {

nit: Consider documenting that the argument should be a state obtained from updStateDisabledConstraints()

---

OpenSim/Moco/MocoProblemRep.h line 349 at r7 (raw file):

    /// model. This function is useful when the model contains user-defined
    /// controllers, which require the controls that are not present in the
    /// optimal control problem to be computed from the model.

nit Consider documenting this in MocoSolution

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 441 at r5 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

I was trying to avoid the unncecessary realizeVelocity() call when getting controls from the ControlDistributor. I found a better way to do with with helper functions (based on your suggestion above).

I imagine you can't avoid realizeVelocity(); that's required for the controls to be computed by model controllers, since a controller can depend arbitrarily on such info. Or at least that's how I think OpenSim's computeControls() works.

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 83 at r3 (raw file):

Previously, nickbianco (Nick Bianco) wrote…

Currently, my design is that the controls vector passed to MocoGoals is always the model controls vector, so there's no need for any index checking from the ControlDistributor. Eventually, we will need a way to design cost functions using the non-model control values (e.g., synergy excitations). I'm not sure if the best way to do this is append the "input" controls to the controls vector passed to all MocoGoals, or create a subset of MocoGoals designed to optimize the input control values (e.g., MocoInputControlGoal). Perhaps we have another suite of goals akin to MocoOutputGoal, (e.g., MocoInputGoal, MocoFinalInputGoal, MocoInputTrackingGoal, etc.). I plan to address this in a subsequent PR.

Yeah sounds like an interesting problem. The "Input" suite sounds clean but like a lot of duplication. Anyway, I'm looking forward to discussing this (if you want) after you merge this PR.

[nickbianco]

Thanks @chrisdembia!

Reviewable status: 42 of 45 files reviewed, 1 unresolved discussion (waiting on @aymanhab and @chrisdembia)

---

OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h line 441 at r5 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

I imagine you can't avoid realizeVelocity(); that's required for the controls to be computed by model controllers, since a controller can depend arbitrarily on such info. Or at least that's how I think OpenSim's computeControls() works.

You can't avoid realizeVelocity() when computing controls from the model, but it's unnecessary when getting them from the ControlDistributor. I hadn't thought of a clever way to do that until your helper function suggestion.

---

OpenSim/Moco/MocoGoal/MocoControlGoal.cpp line 83 at r3 (raw file):

Previously, chrisdembia (Christopher Dembia) wrote…

Yeah sounds like an interesting problem. The "Input" suite sounds clean but like a lot of duplication. Anyway, I'm looking forward to discussing this (if you want) after you merge this PR.

Yes! Let's chat about some possibilities :)