New MocoGoal to minimize jerk

[peiret]

Hi,

My team and I are working on a new MocoGoal that minimizes the jerk (that is, the time-derivative of the acceleration). Minimizing higher-order time derivatives generally helps solver convergence and reduces the noise in the solution.

To do that, we collect the acceleration of each coordinate in MocoGoal::calcIntegrandImpl(), which is supposed to be called for each collocation point (instant of time). Then, we would like to generate a spline for each coordinate acceleration and calculate the derivative (jerk). This last step is implemented in MocoGoal::calcGoalImpl(), but that doesn't work.

We don't seem to get the acceleration in all time instants, but rather a few instants, and sometimes duplicates. We also observed that a clone() of the goal is created for each thread. I asume this complicates the implementation described above since we will have to gather the information collected in all threads...

What would be the best way to collect and sort the acceleration data? Are we missing something?

Thank you in advance!

Here are the goal implementation files:

MocoMinimizeJerkGoal.h

#ifndef OPENSIM_MocoMinimizeJerkGoal_H
#define OPENSIM_MocoMinimizeJerkGoal_H

#include "osimPluginDLL.h"
#include <OpenSim/Moco/MocoWeightSet.h>
#include <OpenSim/Moco/MocoGoal/MocoGoal.h>

namespace OpenSim {

/** Minimize the sum of the absolute value of the jerks raised to a given
exponent, integrated over the phase. The default weight for each jerk is
1.0.
The exponent must be an integer greater than or equal to 2,
and is 2 by default.
If conducting a predictive simulation, you likely want to set
`divide_by_displacement` to true; otherwise, this cost is minimized by not
moving. Dividing by displacement leads to a quantity similar to cost of
transport.

This goal is computed as follows:

\f[
\frac{1}{d} \int_{t_i}^{t_f} \sum_{q \in Q} |\dddot{x_q}(t)|^p ~dt
\f]
We use the following notation:
- \f$ d \f$: displacement of the system, if `divide_by_displacement` is
  true; 1 otherwise.
- \f$ Q \f$: the set of coordinates.
- \f$ \dddot{x_q}(t) \f$: jerk of coordinate \f$ q \f$.
- \f$ p \f$: the `exponent`.

If `p > 2`, we first take the absolute value of the jerk; this is to properly
handle odd exponents.
@ingroup mocogoal */
class OSIMPLUGIN_API MocoMinimizeJerkGoal : public MocoGoal {
OpenSim_DECLARE_CONCRETE_OBJECT(MocoMinimizeJerkGoal, MocoGoal);

public:
    MocoMinimizeJerkGoal();
    MocoMinimizeJerkGoal(std::string name) : MocoGoal(std::move(name)) {
        constructProperties();
    }
    MocoMinimizeJerkGoal(std::string name, double weight)
            : MocoGoal(std::move(name), weight) {
        constructProperties();
    }

    /// Set the exponent.
    void setExponent(int exponent) { set_exponent(exponent); }
    double getExponent() const { return get_exponent(); }

    /** Set if the goal should be divided by the displacement of the system's
    center of mass over the phase. */
    void setDivideByDisplacement(bool tf) { set_divide_by_displacement(tf); }
    bool getDivideByDisplacement() const {
        return get_divide_by_displacement();
    }

protected:
    void initializeOnModelImpl(const Model&) const override;
    void calcIntegrandImpl(
            const IntegrandInput& input, SimTK::Real& integrand) const override;
    void calcGoalImpl(
            const GoalInput& input, SimTK::Vector& cost) const override;
    void printDescriptionImpl() const override;

private:
    void constructProperties();
    OpenSim_DECLARE_PROPERTY(
            exponent, int, "The exponent on jerks; greater than or equal to "
                           "2 (default: 2).");
    OpenSim_DECLARE_PROPERTY(divide_by_displacement, bool,
        "Divide by the model's displacement over the phase (default: "
        "false)");
    mutable std::vector<const OpenSim::Coordinate*> m_model_coordinates;
    mutable std::function<double(const double&)> m_power_function; // no tocar
    mutable std::vector<std::pair<double, double>> m_time_acceleration;
};

} // namespace OpenSim

#endif // OPENSIM_MocoMinimizeJerkGoal_H

MocoMinimizeJerkGoal.cpp

#include "MocoMinimizeJerkGoal.h"

using namespace OpenSim;

MocoMinimizeJerkGoal::MocoMinimizeJerkGoal() { constructProperties(); }

void MocoMinimizeJerkGoal::constructProperties() {
    setAuthors("Albert Peiret, Marcel Jané");
    constructProperty_exponent(2);
    constructProperty_divide_by_displacement(false);
}

void MocoMinimizeJerkGoal::initializeOnModelImpl(const Model& model) const {
    // Check if the exponent value is correct
    OPENSIM_THROW_IF_FRMOBJ(get_exponent() < 2, Exception,
            "Exponent must be 2 or greater.");
    int exponent = get_exponent();

    // The pow() function gives slightly different results than x * x. On Mac,
    // using x * x requires fewer solver iterations.
    if (exponent == 2) {
        m_power_function = [](const double& x) { return x * x; };
    } else {
        m_power_function = [exponent](const double& x) {
            return pow(std::abs(x), exponent);
        };
    }

    for (const OpenSim::Coordinate& coord : model.getComponentList<Coordinate>()) {
        m_model_coordinates.push_back(&coord);
    }
    m_time_acceleration.clear();
    setRequirements(1, 1, SimTK::Stage::Acceleration);
}

void MocoMinimizeJerkGoal::calcIntegrandImpl(
        const IntegrandInput& input, SimTK::Real& integrand) const {

    integrand = 0;
    const SimTK::State& state = input.state;
    const double time = state.getTime();
    getModel().realizeAcceleration(state);
    for (int icoord = 0; icoord < (int)m_model_coordinates.size(); ++icoord) {
        const double acceleration = m_model_coordinates[icoord]->getAccelerationValue(state);
        m_time_acceleration.push_back({ time, acceleration });
    }
}

void MocoMinimizeJerkGoal::calcGoalImpl(
        const GoalInput& input, SimTK::Vector& cost) const {

    double integrand = 0;
    std::sort(m_time_acceleration.begin(), m_time_acceleration.end());

    /*
    Here, is where we want to create the spline with the accelerations
    */

    cost[0] = integrand;
    if (get_divide_by_displacement()) {
        cost[0] /=
                calcSystemDisplacement(input.initial_state, input.final_state);
    }
    m_time_acceleration.clear();
}

void MocoMinimizeJerkGoal::printDescriptionImpl() const {
    int exponent = get_exponent();
//     log_cout("        Minimize Jerk Goal, weight: {}", weight);
    log_cout("        Minimize Jerk Goal, using exponent: {}", exponent);

}

[nickbianco]

Hi @peiret, thanks for sharing your prototype. Unfortunately, I don't think this approach will work. Populating the m_time_acceleration variable like this will produce inconsistent results since both calcIntegrandImpl and calcGoalImpl get called multiple times per iterate during an optimization, meaning that you're duplicating acceleration values or clearing them too early.

The way MocoGoal is designed, you can only use the inputs provided via const GoalInput& input. The correct way to minimize jerk would be to introduce another layer of dynamics such that j = da/dt, j is jerk modeled using a control variable, and a is acceleration, now a state variable. You could then simply minimize the jerk controls.

This is unfortunately significantly more complicated then just adding a new MocoGoal, but we could mimic the way acceleration controls are handled when using implicit multibody dynamics.