Increase speed of tools when dealing with large experimental files

[jimmyDunne]

Improvements in speed could be made by reading initially reading in a small block of data and beginning tool while a second process continues to read data as needed.

[aymanhab]

@mitkof6 mentioned refactoring the tools to allow for streaming mode where data-frames are read/processed one at a time rather than read entirely in advance. This would lend itself to realtime processing and speed startup. Any reference code you can share @mitkof6 Thanks

[mitkof6]

The approach that I followed was to re-implement all the tools from scratch. I wanted to have simple, light-weighted classes that can be easily maintained, extended and used like signal processing blocks. I started examining the rtosim repository:

https://github.com/RealTimeBiomechanics/rtosim

However, as you can see, even for IK the code becomes obsolete, hard to maintain or understand:

https://github.com/RealTimeBiomechanics/rtosim/tree/master/lib/InverseKinematics

Therefore, I decided that it would take me less time to directly work with the simbody methods. Here is an implementation of IK that works with IMU data (OpenSim 3.3):

class Simulation_API InverseKinematics {
 public:
    struct MarkerTask {
        std::string name;
	std::string marker;
        double weight;
    };
    struct IMUTask {
	std::string name;
	std::string body;
	SimTK::Rotation orientation;
	double weight;
    };
    struct Input {
	double t;
	SimTK::Array_<SimTK::Vec3> markerObservations;
	SimTK::Array_<SimTK::Rotation> imuObservations;
    };
    struct Output {
	double rms;
	double t;
	SimTK::Vector q;
    };
    OpenSim::Model model;
    SimTK::State state;
    SimTK::ReferencePtr<SimTK::Assembler> assembler;
    SimTK::ReferencePtr<SimTK::Markers> markerAssemblyConditions;
    SimTK::ReferencePtr<SimTK::OrientationSensors> imuAssemblyConditions;
    bool assembled;
 public:
    InverseKinematics(std::string modelFile,
		      double constraintsWeight,
                      const std::vector<MarkerTask>& markerTasks,
		      const std::vector<IMUTask>& imuTasks);
    Output solve(const Input& input);
};

InverseKinematics::InverseKinematics(string modelFile,
				     double constraintsWeight,
                                     const vector<MarkerTask>& markerTasks,
				     const vector<IMUTask>& imuTasks)
    : model(modelFile), assembled(false) {   
    // initialize model and assembler
    state = model.initSystem();
    assembler = new Assembler(model.getMultibodySystem());
    // assembler->setAccuracy(1e-3);
    // assembler->setErrorTolerance(1e-3);
    assembler->setSystemConstraintsWeight(constraintsWeight);

    // populate assembly conditions for markers
    markerAssemblyConditions = new Markers();
    Array_<string> markerObservationOrder;
    for (const auto& task : markerTasks) {
        int markerIndex = model.getMarkerSet().getIndex(task.marker);
	if (markerIndex < 0){
            THROW_EXCEPTION("marker: "+ task.marker + " does not exist in model");
        }
        const auto& marker = model.getMarkerSet()[markerIndex];
        const auto& mobod = model.getMatterSubsystem().
            getMobilizedBody(marker.getBody().getIndex());
        markerAssemblyConditions->addMarker(task.name,
                                            mobod,
                                            marker.getOffset(),
                                            task.weight);
        markerObservationOrder.push_back(task.name);
    }
    markerAssemblyConditions->defineObservationOrder(markerObservationOrder);
    if (markerObservationOrder.size() != 0) {
	assembler->adoptAssemblyGoal(markerAssemblyConditions);
    }

    // populate assembly conditions for markers
    imuAssemblyConditions = new OrientationSensors();
    Array_<string> imuObservationOrder;
    for (const auto& task : imuTasks) {
        int bodyIndex = model.getBodySet().getIndex(task.body);
	if (bodyIndex < 0){
	    THROW_EXCEPTION("body: "+ task.body + " does not exist in model");
        }
        const auto& body = model.getBodySet()[bodyIndex];
        const auto& mobod = model.getMatterSubsystem().
            getMobilizedBody(body.getIndex());
        imuAssemblyConditions->addOSensor(task.name,
					  mobod,
					  task.orientation,
					  task.weight);
        imuObservationOrder.push_back(task.name);
    }
    imuAssemblyConditions->defineObservationOrder(imuObservationOrder);
    if (imuObservationOrder.size() != 0) {
	assembler->adoptAssemblyGoal(imuAssemblyConditions);
    }

    assembler->initialize(state);
}

InverseKinematics::Output InverseKinematics::solve(
    const InverseKinematics::Input& input) {
    state.updTime() = input.t;
    markerAssemblyConditions->moveAllObservations(input.markerObservations);
    imuAssemblyConditions->moveAllObservations(input.imuObservations);
    double rms;
    if (assembled) {
        rms = assembler->track();
    } else {
        rms =assembler->assemble();
        assembled = true;
    }
    assembler->updateFromInternalState(state);
    return InverseKinematics::Output{rms, input.t, state.getQ()};
}

I have different useful utilities for pre-processing experimental data and interface the sources with the tools. In this implementation, you can call solve iteratively and I did not process the whole .trc at once.

I had to have the system (real-time estimation of joint reaction forces) working in one month due to a project review, so most of the functions/members are public. I would like to port everything for v4.0 and refactor my code so that it can follow a proper design.

[mitkof6]

Each tool had to have its own reference of the Model, because the model and state are actually coupled. In my context, I was solving different problems (IK, ID, SO, JR) together, therefore, the realization of the state might affect the behavior of the tools. I find that if the tool is somehow decoupled, the code can be parallelized, improved and maintained easily.