Page Comparison

// Compute the number of optimization parameters for the model.
_numActuators = _cat.getActuators().getSize();
int numParameters = _numActuators * _numOptimSplinePoints;
setNumParameters(numParameters);

// Size the vector of best-yet splines.
_splinesBestYet.resize(_numActuators);

// Add a PrescribedController to the model.
using OpenSim::PrescribedController;
PrescribedController * flipController = new PrescribedController();
flipController->setName("flip_controller");

// Set the PrescribedController to control all actuators.
flipController->setActuators(_cat.getActuators());

// Add the PrescribedController to the model.
_cat.addController(flipController);

// Create SimmSpline's for each actuator and push into vector.
double indexToTimeInSeconds =
    _duration / (double)(_numOptimSplinePoints - 1);
for (int i = 0; i < _numActuators; i++)
{
    // Create a spline function for the current actuator.
    _splines.push_back(new OpenSim::SimmSpline());
    // Name this spline with the name of the current actuator.
    _splines[i]->setName(_cat.getActuators().get(i).getName());
    // Add the correct number of points to this spline.
    for (int j = 0; j < _numOptimSplinePoints; j++)
    {
        // Scale the time points appropriately.
        double thisTime = (double)j * indexToTimeInSeconds;
        _splines[i]->addPoint(thisTime, 0.0);
    }
    // Tell the controller about this function.
    flipController->prescribeControlForActuator(i, _splines[i]);
} 

SimTK::State& initState = _cat.initSystem();

// Construct an integrator.
SimTK::RungeKuttaMersonIntegrator integrator(_cat.getMultibodySystem());
integrator.setAccuracy(1.0e-6);

// Construct a manager to run the integration.
OpenSim::Manager manager(_cat, integrator);
_cat.getMultibodySystem().realize(initState, SimTK::Stage::Acceleration);

// Integrate from initial time to final time
manager.setInitialTime(0.0);
manager.setFinalTime(_duration);
manager.integrate(initState);

// Will be writing to a log while constructing the objective function.
if (_objectiveCalls % _outputPeriod == 0)
    _optLog << _objectiveCalls;

// Copy the model's state. We need a consistent state to access the
// model's configuration and related speeds.
SimTK::State aState = initState;
_cat.getMultibodySystem().realize(aState, SimTK::Stage::Acceleration);
const OpenSim::CoordinateSet& coordinates = _cat.getCoordinateSet();
double roll = coordinates.get("roll").getValue(aState);
double rollRate = coordinates.get("roll").getSpeedValue(aState);
double rollAccel = coordinates.get("roll").getAccelerationValue(aState);
double twist = coordinates.get("twist").getValue(aState);
double twistRate = coordinates.get("twist").getSpeedValue(aState);
double twistAccel = coordinates.get("twist").getAccelerationValue(aState);
double hunch = coordinates.get("hunch").getValue(aState);
double pitch = coordinates.get("pitch").getValue(aState);
double pitchRate = coordinates.get("pitch").getSpeedValue(aState);
double pitchAccel = coordinates.get("pitch").getAccelerationValue(aState);

// --------------------------------------------------------------------
f = 0;
f += anteriorLegsDownTerm(roll, rollRate, rollAccel);
f += posteriorLegsDownTerm(twist, twistRate, twistAccel);
f += sagittalSymmetryTerm(hunch, pitch, pitchRate, pitchAccel);
// --------------------------------------------------------------------

/**
 * @brief Prints the current model to the given filename.  The file will
 * be located in the directory containing the log file for this
 * optimization run.
 * */
void printModel(std::string filename) const
{
    _cat.print(_name + "/" + filename);
}
/**
 * @brief Serializes the current set of functions used for the actuators.
 * The file will be located in the directory containing the log file
 * for this optimization run.
 * */
void printPrescribedControllerFunctionSet(std::string filename) const
{
    // Create the FunctionSet that we'll then serialize.
    OpenSim::FunctionSet fset;
    fset.setSize(_splines.size());
    for (unsigned int iFcn = 0; iFcn < _splines.size(); iFcn++)
    {
        fset.insert(iFcn, *_splines[iFcn]);
    }
    fset.print(_name + "/" + filename);
}
/**
 * @brief Serializes the set of functions associated with the best-yet
 * value of the objective function. The file will be located in the
 * directory containing the log file for this optimization run.
 *
 * @param (nondimensionalize) Divide spline values by minControl for the
 * actuator, if value and minControl are negative, and by maxControl, if
 * value is positive. Once nondimensionalized, this FunctionSet can be used
 * as initial parameters.
 * */
void printBestYetPrescribedControllerFunctionSet(std::string filename,
        bool nondimensionalize=false) const
{
    using OpenSim::SimmSpline;
    // Create the FunctionSet that we will then serialize.
    OpenSim::FunctionSet fset;
    fset.setSize(_splinesBestYet.size());
    for (unsigned int iFcn = 0; iFcn < _splinesBestYet.size(); iFcn++)
    {
        fset.insert(iFcn, _splinesBestYet[iFcn]);
        if (nondimensionalize)
        { // Go through each y-value and nondimensionalize based on its sign.
            // Max/min for all spline points for the i-th actuator.
            double minControl = _cat.getActuators().get(iFcn).getMinControl();
            double maxControl = _cat.getActuators().get(iFcn).getMaxControl();
            for (int iPts = 0; iPts < _numOptimSplinePoints; iPts++)
            {
                SimmSpline * fcn = dynamic_cast<SimmSpline *>(&fset.get(iFcn));
                double dimValue = fcn->getY(iPts);
                double nonDimValue = 0;
                if (dimValue < 0 && minControl < 0)
                {
                    nonDimValue = -dimValue / minControl;
                }
                else
                {
                    nonDimValue = dimValue / maxControl;
                }
                fcn->setY(iPts, nonDimValue);
            }
        }
    }
    fset.print(_name + "/" + filename);
}

// Reference to the FlippinFelinesOptimizerTool.
OpenSim::FlippinFelinesOptimizerTool & _tool;

// Simulation runtime (seconds).
double _duration;

// Model containing the attributes described in this class' description
mutable OpenSim::Model _cat;

// Number of actuators in the model.
int _numActuators;

// Number of points used to define splines for actuator control.
int _numOptimSplinePoints;

// Vector of splines used in the PrescribedController.
std::vector<OpenSim::SimmSpline *> _splines;

// Vector of splines that gave the best objective value yet.
mutable std::vector<OpenSim::SimmSpline> _splinesBestYet;

// Number of calls to the objective function.
mutable int _objectiveCalls;

// Name of results directory.
std::string _name;

// Log for recording data during optimization
mutable std::ofstream _optLog;

// Period for how often objective terms are printed to the log.
static const int _outputPeriod = 100;

// Best (lowest) value of the objective function, for logging.
mutable double _objectiveFcnValueBestYet;

// To aid with conservative printing of best yet actuation:
mutable bool _lastCallWasBestYet;
mutable bool _thisCallIsBestYet;