Efficiently calculating a subset of a system's body and mobility forces

[nickbianco]

I am working on a new feature for OpenSim Moco that would allow users to track a reference set of generalized coordinate forces in a trajectory optimization problem. The OpenSim InverseDynamicsTool uses SimbodyMatterSubsystem::calcResidualForce() to compute generalized coordinate forces by internally making a copy of the model and disabling forces that should be excluded from the anaylsis (usually all forces except external loads, e.g., from ground reaction force data).

In Moco, it would be preferable to not require a copy of the model since all classes that implement cost function terms already have a reference to the original model. Instead, we ideally should only need to retrieve the set of body and mobility forces needed to compute the generalized forces from inverse dynamics. Force::calcForceContribution has the desired functionality, but, as noted in the documentation, it requires resizing and zeroing out passed in body and mobility force arrays and is therefore not the best choice for use in optimization.

Desired workflow

The desired workflow to compute generalized forces within Moco's trajectory optimization framework would look as follows:

// Non-modifiable inputs from the trajectory optimization framework.
const State& state = input.state;
const OpenSim::Model& model = input.model;

// Get the generalized accelerations.
model.realizeAcceleration(state);
const Vector& udot = state.getUDot();

// Get a subset of the applied body and mobility forces in the model. 
Vector subsetAppliedMobilityForces = ?
Vector_<SpatialVec> subsetAppliedBodyForces = ?

// Compute the residual mobility forces (i.e., the inverse dynamics generalized forces).
const SimbodyMatterSubsystem& matter = model.getMatterSubsystem();
Vector residualMobilityForces(state.getNU());
matter.calcResidualForceIgnoringConstraints(state, 
        subsetAppliedMobilityForces, subsetAppliedBodyForces, 
        udot, residualMobilityForces);

Proposed method

One potential option is to add a method to GeneralForceSubsystemRep to calculate the subset of desired forces based on a list of ForceIndexes:

GeneralForceSubsystemRep::calcForces(const State& state, 
        const Array_<ForceIndex>& forceIndexes, 
        Vector_<SpatialVec>& bodyForces, 
        Vector& mobilityForces) {

    // appropriate asserts/checks here

    Vector_<Vec3> particleForces; // unused
    for (const auto& forceIndex : forceIndexes) {
        if (isForceDisabled(state, forceIndex)) continue;
        getForce(forceIndex).calcForce(state, bodyForces, particleForces, mobilityForces);
    }
}

GeneralForceSubsystem::calcForces(const State& state, 
        const Array_<ForceIndex>& forceIndexes, 
        Vector_<SpatialVec>& bodyForces, 
        Vector& mobilityForces) {
    getRep.calcSubsetForces(state, forceIndexes, bodyForces, mobilityForces);
}

Even if we need to resize/zero the input arrays in this implementation, it would only need to be done once for all forces. If it's possible to skip the zeroing step (and rename to method to something like addInForces or something), that could be even better.

If an addition like this make sense to add to the API, I'd be happy to submit pull request after agreeing upon an implementation.

[nickbianco]

It seems like I might be able to leverage the existing CalcForcesTask and ParallelExecutor within GeneralForceSubsystemRep by setting enabledNonParallelForces and/or enabledParallelForces to reflect the forces I'd like to include in the computation. Taking a closer look now.

[nickbianco]

I decided to open a PR for this, since I had relatively straight-forward implementation working for me. I decided to keep it simple and avoid CalcForcesTask for now, since there's some caching steps in there that I don't fully understand yet.