opensim-4.0-pseudocode

deserialize-state-save-calculation.py

from opensim import *
model = Model('mymodel.osim')
states = StateTrajectory('mystates.osimstates')

comtraj = TimeSeriesTableVec3(states.size(), 2)
comtraj.setColumnLabels(['com_position', 'com_velocity'])
for state in states:
    comtraj.appendRow(state.getTime(), [
            model.getMassCenter(state),
            model.getMassCenterVelocity(state),
            ]
        )
comtraj.print('com_history.sto')
comtraj.print('com_history.mat')

# OR
MATWriter(comtraj, 'com_history.mat').write()
    

inverse-dynamics.cpp

/** Basic component that could be embedded within a Model. */
class InverseDynamics : public Component {
public:
    void constructInputs() {
        constructInput("udot", SimTK::Vector);
    }

    void constructOutputs() {
        constructOutput("residual", SimTK::Vector, &solve);
    }

    void constructConnectors() {
        constructConnector<Model>("model"); // TODO maybe unnecessary.
    }

    SimTK::Vector solve(const SimTK::State& s) const {
        const auto& smss =
            updConnector<Model>("model").getConnectee().getMatterSubsystem();

        // Get appliedMobilityForces, appliedBodyForces from MultibodySystem.

        Vector residualMobilityForces;

        smss.calcResidualForceIgnoringConstraints(s, 
                appliedMobilityForces, appliedBodyForces, getInput(s, "udot"),
                residualMobiityForces);

        return residualMobilityForces;
    }
};

/** Try to optimize memory. */
class InverseDynamics : public Component {
public:
    void constructInputs() {
        constructInput("udot", SimTK::Vector);
    }

    void constructOutputs() {
        constructOutput("residual", const SimTK::Vector&,
                SimTK::Stage::Velocity,
                &solve);
    }

    void constructConnectors() {
        constructConnector<Model>("model");
    }

    const SimTK::Vector& solve(const SimTK::State& s) const {
        const auto& smss =
            updConnector<Model>("model").getConnectee().getMatterSubsystem();

        // Get appliedMobilityForces, appliedBodyForces from MultibodySystem.

        if (!m_residualCache.isValid(s)) {
            auto& residual = m_residualCache.updValue(s);
            smss.calcResidualForceIgnoringConstraints(s, 
                    appliedMobilityForces, appliedBodyForces,
                    getInput(s, "udot"),
                    residual);
            m_residualCache.markAsValid(s);
        }

        return m_residualCache.getValue(s);
    }

    void extendAddToSystem(SimTK::MultibodySystem& system) const {
        const_cast<InverseDynamics*>(this)->m_residualCache =
            Measure_<Vector>::Result(_system->updDefaultSubsystem(),
                    Stage::Velocity, Stage::Acceleration);
    }
private:
    Measure_<Vector> m_residualCache;
};

/** Specify inputs, outputs, and connectors with macros. */
class InverseDynamics : public Component {
public:

    OpenSim_DECLARE_INPUT(udot, SimTK::Vector, "Generalized accelerations. "
            "Length must be number of velocity degrees of freedom.");

    OpenSim_DECLARE_OUTPUT(residual, SimTK::Vector, SimTK::Stage::Velocity,
            "Generalized forces necessary to satisfy Newton's 2nd law. "
            "Length is number of velocity degrees of freedom.");

    OpenSim_DECLARE_CONNECTOR(model, Model,
            "The model to use for the inverse dynamics calculation.");

    const SimTK::Vector& solve(const SimTK::State& s) const {
        const auto& smss =
            updConnector<Model>("model").getConnectee().getMatterSubsystem();

        // Get appliedMobilityForces, appliedBodyForces from MultibodySystem.

        Vector residualMobilityForces;

        smss.calcResidualForceIgnoringConstraints(s, 
                appliedMobilityForces, appliedBodyForces, get_udot(s),
                m_residualMobiityForces);

        return residualMobilityForces;
    }
};

/** The user provides applied forces. Should we support optional inputs, etc? */
class InverseDynamics : public Component {
public:

    OpenSim_DECLARE_INPUT(udot, SimTK::Vector, "Generalized accelerations. "
            "Length must be number of velocity degrees of freedom.");
    OpenSim_DECLARE_INPUT(applied_mobility_forces, SimTK::Vector, 
            "Generalized forces to consider as applied to the model. "
            "Length should be number of mobilities.");
    OpenSim_DECLARE_INPUT(applied_body_forces, SimTK::Vector_<SpatialVec>,
            "Body forces to consider as applied to the model. "
            "Length should be number of bodies.");

    OpenSim_DECLARE_OUTPUT(residual, SimTK::Vector, SimTK::Stage::Velocity,
            &solve,
            "Generalized forces necessary to satisfy Newton's 2nd law. "
            "Length is number of velocity degrees of freedom.");

    OpenSim_DECLARE_CONNECTOR(model, Model,
            "The model to use for the inverse dynamics calculation.");

    const SimTK::Vector& solve(const SimTK::State& s) const {
        const auto& smss =
            updConnector<Model>("model").getConnectee().getMatterSubsystem();

        Vector residualMobilityForces;

        smss.calcResidualForceIgnoringConstraints(s, 
                get_applied_mobility_forces(s), get_applied_body_forces(s),
                get_udot(s),
                residualMobiityForces);
    }
};

/** Embedding InverseDynamics inside a controller. */
class FeedforwardController : public Controller {
public:
    OpenSim_DECLARE_PROPERTY(kp, double, "Position gain.");
    OpenSim_DECLARE_PROPERTY(kv, double, "Speed gain.");
    void constructInputs() {
        constructInput("q_des", SimTK::Vector);
        constructInput("u_des", SimTK::Vector);
    }
    void constructOutput() {
        constructOutput("udot_des", SimTK::Vector, SimTK::Stage::Velocity,
                &desiredAcceleration);
    }
    SimTK::Vector desiredAcceleration(const SimTK::State& s) {
        // TODO I can't use udot here?
        return s.getUDot() + get_kp() * (getInput("q_des") - s.getQ()) +
                             get_kv() * (getInput("u_des") - s.getU());
    }
    void computeControls(const SimTK::State& s, SimTK::Vector& controls) {
        controls = m_invdyn.getOutputValue(s, "residual");
        
    }
    void extendConnectToModel(Model& model) {
        m_invdyn.updConnector<Model>("model").connect(model); // TODO
        m_invdyn.updInput("udot").connect(getOutput("udot_des"));
    }
private:
    InverseDynamics m_invdyn;
};
```xml
<ForwardTool>
    <model_filename>mymodel.osim</model_filename>
    <ControllerSet name="Controllers">
        <objects>
            <FeedforwardController>
                <kp>20</kp> <kv>100</kv>
                <q_des>
                    <connected_to_name>/desired_joint_angles</connected_to_name>
                </q_des>
                <u_des>
                    <connected_to_name>/desired_speeds</connected_to_name>
                </u_des>
            </FeedforwardController>
        </objects>
    </ControllerSet>
    <components>
        <DataSource name="desired_joint_angles">
            <filename>inverse_kinematics_results.sto</filename>
        </DataSource>
        <NumericalDifferentiator name="desired_speeds">
            <method>forward</method>
            <order>2</order>
            <input>
                <connected_to_name>/desired_joint_angles</connected_to_name>
            </input>
        </NumericalDifferentiatior>
    </components>
</ForwardTool>
```

/** Runs the InverseDynamics component on an entire motion. Programmatic. */
class InverseDynamicsStudy : public Object {
    OpenSim_DECLARE_PROPERTY("q", DataSource);
    OpenSim_DECLARE_PROPERTY("residual", DataSink);
    OpenSim_DECLARE_PROPERTY(model_filename, Pathname, "");
    void run() const {
        InverseDynamics invdyn;

        // Convert labeled columns to a single vector.
        // Takes a model and any Component who should have an output for each
        // unconstrained coordinate. This class can handle generic error
        // handling for missing data for unconstrained coordinates.
        CoordinateMultiplexer mux(model, get_q());
        // TODO or use a more generic multiplexer.

        // Compute 2nd derivative numerically.
        NumericalDifferentiator<Vector> diff;
        diff.updInput("input").connect(mux.getOutput("output"));
        diff.setOrder(2);

        // Hook up ID's udot input to the udot produced by the differentiator.
        invdyn.updInput("udot").connect(diff.getOutput("output"));

        // Hook up the residual DataSink to ID's output.
        // MobilizerDemultiplexer writes to a separate input for each element
        // of the vector input. This requires multiconnectors.
        MobilizerDemultiplexer demux(model, get_residual());
        demux.updInput("input").connect(invdyn.getOutput("residual"));

        // TODO creates a state trajectory to the best of its ability based on
        // just joint angles.
        StateTrajectory states = StateTrajectory(get_q());
        //StateTrajectory states =
        //    StateTrajectory::fromCoordinates(get_q(), "GCVSpline");

        for (const auto& state : states) {
            // This causes the DataSink to be filled up.
            auto residual = invdyn.solve(state);
            // TODO actually, should be something like:
            upd_residual().update(states); // TODO see BTK.
            model.realizeReport(state); // TODO this probably makes more sense.
        }

        // Without having to do anything here, the DataSink has already written
        // to a file. TODO should there be a "finalize" step to invoke the
        // file-writing?
    }
};

```
<InverseDynamicsStudy>
    <model_filename>C:/data/mymodel.osim</model_filename>
    <q>
        <DataSource>
            <filename>inverse_kinematics.sto</filename>
        </DataSource>
    </q>
    <residual>
        <DataSink>
            <filename>inverse_dynamics_solution.sto</filename>
        </DataSink>
    </residual>
</InverseDynamicsStudy>
```

/** Quick custom InverseDynamics in Python. */
// TODO too complicated?
// Create convenience "ConstantVector" class.
```python
id = InverseDynamics()
udot = ConstantVector(Vector(1, 2, 3, 4, 5))
id.getInput("udot").connect(udot.getOutput("output"))
id.updConnector("model").connect(model) TODO non-templatized.
state = model.initSystem()
residual = id.solve(state)
```

reporting.cpp

/** Printing results to a file can be performed via a DataSink component. */

int main() {

    Model model("mymodel.osim");
    ForwardStudy study;

    // Add components to the study.
    study.add(model);
    DataSink sink;
    sink.setName("muscle_activity");
    sink.setDataWriter(HDF5Writer("muscle_activity.h5", "/activation"));
    // TODO could not write continuously to an HDF5 table; must do that once at
    // the end.

    // Define what the inputs to the sink are; that is, what to save.
    sink.getMultiInput().append("soleus_r/activation");
    // Add all muscle activation to the output; with globbing?? :)
    sink.getMultiInput().append("*/activation");
    study.add(sink);

    // This runs the integration and fills up the DataSink.
    study.run();

    // Use the result without having to read a file! Very important***
    const auto& sink = study.get("muscle_activity");
    const TimeSeriesDataTable& dat = sink.getTable();
    const auto soleus_act = dat.getColumn("soleus_r/activation");
    cout << "Peak soleus activity: " << SimTK::max(soleus_act) << endl;
    cout << "Average soleus activity: " << SimTK::mean(soleus_act) << endl;

    sink.getTable().print();

}