Functionality to specify a specific axis when using MocoAverageSpeedGoal

[nicos1993]

I was having some strange MocoTrack simulations in that they would converge... but they would start further forward at the beginning than at the end (so go backwards). I looked into MocoAverageSpeedGoal and noticed it was calculating the norm of the COM displacement to enforce the constraint... explains why solutions stepping back are still feasible or even solutions that fly.

When using this goal I wanted to simply use the anterior-posterior COM displacement, rather than the norm, so I have exposed a new property axis_component to do so. If the user sets -1 for the property it performs the norm approach.

I found greater success with getting reasonable tracking simulations using approach.

---

This change is 

[nickbianco]

Thanks @nicos1993! I really like this idea.

I left a handful of comments about simplifying the implementation and making the interaction of the properties of MocoAverageSpeedGoal a bit clearer.

@nickbianco reviewed 2 files and all commit messages, and made 8 comments.
Reviewable status: all files reviewed, 6 unresolved discussions (waiting on nicos1993).

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 477 at r1 (raw file):

    /// Calculate the displacement of the system's center of mass over the
    // phase.

Make sure all comments have three slashes so they appear in Doxygen:

Suggestion:

    /// phase.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 480 at r1 (raw file):

    double calcSystemDisplacement(const GoalInput& input) const;
    // Calculate the displacement of the system's center of mass over the
    // phase for a specific axis/direction.

"axis" and "direction" have different connotations (see my comment below).

It might be better to have a method like SimTK::Vec3 calcSystemDisplacementVector(const GoalInput& input) which returns the value const SimTK::Vec3 delta = comFinal - comInitial and then the average speed goal can used it as needed. So you would end up with something like:

SimTK::Vec3 MocoGoal::calcSystemDisplacementVector(const GoalInput& input) const {
    const SimTK::Vec3 comInitial =
            getModel().calcMassCenterPosition(input.initial_state);
    const SimTK::Vec3 comFinal =
            getModel().calcMassCenterPosition(input.final_state);
    return comFinal - comInitial;
}

double MocoGoal::calcSystemDisplacement(const GoalInput& input) const {
    // TODO: use squared norm for convexity.
    return calcSystemDisplacementVector(const GoalInput& input).norm();
}

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 485 at r1 (raw file):

    // Euclidean norm.
    double calcSystemDisplacement(const GoalInput& input,
            int axisComponent) const;

See comment above.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 591 at r1 (raw file):

    OpenSim_DECLARE_PROPERTY(axis_component, int,
            "The component of the displacement vector to use: 0=X, 1=Y, 2=Z, "
            "or -1 (default) for the full 3D Euclidean norm.");

Perhaps a better name here is displacement_axis or displacment_direction? A few more clarifying comments here would be helpful, i.e., how does this setting affect the average speed calculation?

Also, it's not totally unclear from the definitions how these two properties interact with each other. For example, if you wanted to move in the negative-x direction. You would need to set the axis to 0, and provide a negative value for the desired average speed. I wonder if it would be clearer to keep the notion of desired average speed as an absolute value (even though we don't explicitly enforce that), and provide the direction as part of the displacement axis (i.e., "negative-x" or "positive-z"). For reference, we do this in MocoStepLengthAsymmetryGoal.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 607 at r1 (raw file):

        const double displacement =
                calcSystemDisplacement(
                    input, get_axis_component());

Following my comments about calcSystemDisplacementVector as an alternative above.

double duration = calcDuration(input);
if (axis < 0) {
    values[0] = get_desired_average_speed() - (calcSystemDisplacement(input);/ duration);
} else {
    const SimTK::Vec3 displacementVector = calcSystemDisplacementVector(input);
    values[0] = get_desired_average_speed() - (sign * displacementVector[axis] / duration);
} 

Where sign and axis would be pre-determined based on the displacement_direction (or whatever we call it) property.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 616 at r1 (raw file):

        // Validate axis_component value at initialization time.
        const int axisComponent = get_axis_component();

FYI, you can use Object::checkPropertyValueIsInSet for checks like this:

checkPropertyValueIsInSet(getProperty_axis_component(), {-1, 0, 1, 2});

---

OpenSim/Moco/MocoGoal/MocoGoal.cpp line 66 at r1 (raw file):

    if (axisComponent == -1) { return delta.norm(); }
    return delta[axisComponent];
}

See comments above about deferring the displacement axis select down to MocoAverageSpeedGoal.

[nicos1993]

Thanks for the comments @nickbianco ! Your suggestions made it much cleaner overall! Let me know what you think!

Actually... I didn't use checkPropertyValueIsInSet... I think that looks cleaner...

[nickbianco]

Thanks @nicos1993! Definitely looking better. However, now that I've seen my suggestions implemented, I've changed my mind about how to specify the displacement direction. I think it would be better to not change the behavior of desired_average_speed (i.e., keep directionality) and define a displacement_axis rather than displacement_direction. With good documentation, this is just as easy to understand for users and would simplify the implementation a bit.

@nickbianco reviewed 2 files and all commit messages, made 6 comments, and resolved 3 discussions.
Reviewable status: all files reviewed, 7 unresolved discussions (waiting on nicos1993).

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 477 at r2 (raw file):

    /// Calculate the norm displacement of the system's center of mass
    /// over the phase.

Suggestion:

    /// Calculate the displacement of the system's center of mass
    /// (i.e., the norm of the difference between the initial and 
    /// final center of mass position) over the phase.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 480 at r2 (raw file):

    double calcSystemDisplacement(const GoalInput& input) const;
    /// Calculate the displacement of the system's center of mass over the
    /// phase.

Suggestion:

    /// Calculate the displacement vector of the system's center of mass 
    /// (i.e., the difference between the initial and final center of 
    /// mass position) over the phase.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 591 at r2 (raw file):

            "for the full 3D Euclidean norm of the CoM displacement. Note "
            "that 'desired_average_speed' should always be positive; the "
            "sign of the direction is embedded in this property.");

I would make this an optional property and remove the "norm" option. Then, you could set m_direction_index = -1 if no direction is specified and avoid the string comparison in calcGoalImpl() below.

Also, now that I think about it, I'm not sure I want to displacement direction with this property since that would change the meaning of desired_average_speed. I thought it would be a good idea until seeing it implemented. Sorry to switch things up on you, but let's revert and make this property displacement_axis instead. I think it's then fine to make the property type int. Then, instead of an optional property, you could just make the default value -1, which will indicate to compute the displacement vector norm.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 626 at r2 (raw file):

                "norm"};
        checkPropertyValueIsInSet(
                getProperty_displacement_direction(), directions);

FYI you can combine these two lines, e.g.,

checkPropertyValueIsInSet(
                 getProperty_displacement_direction(), 
                 {"positive-x", "positive-y", "positive-z", 
                  "negative-x", "negative-y", "negative-z", "norm"});

Based on my suggestions above, this might now look like

checkPropertyValueIsInSet(getProperty_displacement_axis(), 
                 {-1, 0, 1, 2});

---

OpenSim/Moco/MocoGoal/MocoGoal.cpp line 53 at r2 (raw file):

double MocoGoal::calcSystemDisplacement(const GoalInput& input) const {
    // Default behaviour: full 3D Euclidean norm of CoM displacement.

Can you replace this comment with the original one:

Suggestion:

    // TODO: instead use squared norm for convexity.

[nicos1993]

Thanks for suggestions Nick! I have tried to implement them! I removed the mutable variables from before as they didn't seem needed anymore!

[nickbianco]

Sorry for the delay @nicos1993! This one got lost in the shuffle for me. Just a few more comments to address, then we should be good to merge.

@nickbianco reviewed 2 files and all commit messages, made 4 comments, and resolved 7 discussions.
Reviewable status: all files reviewed, 3 unresolved discussions (waiting on nicos1993).

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 589 at r3 (raw file):

    OpenSim_DECLARE_PROPERTY(displacement_axis, int,
            "Specify an axis to compute the average desired speed. "
            "Accepted values are 0, 1, 2 or -1 (norm - default).");

This property description needs a bit more detail, e.g.:

Suggestion:

    OpenSim_DECLARE_PROPERTY(displacement_axis, int,
            "Specify which axis of the displacement vector is used "
            "to compute the average desired speed. Accepted values "
            "are 0, 1, 2 or -1 (default). A value of -1 indicates "
            "to compute the displacement using the 2-norm of the "
            "displacement vector.");

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 604 at r3 (raw file):

        // Calculate average gait speed.
        double displacement;
        if (get_displacement_axis() == -1) {

Let's cache this check with a boolean member variable via initializeOnModelImpl(). We usually try to avoid calls to property accessors inside functions that will be called repeatedly.

---

OpenSim/Moco/MocoGoal/MocoGoal.h line 610 at r3 (raw file):

                    calcSystemDisplacementVector(input);
            displacement =
                    displacementVector[get_displacement_axis()];

Same here: store the axis integer with a member variable.

[nickbianco]

@nicos1993 did you want to proceed with this PR?