Page Comparison

// Parse inputs
// --------------------------------------------------------------------
_name = _tool.get_results_directory();
_cat = OpenSim::Model(_tool.get_model_filename());

// Disable coordinate limit forces?
if (!_tool.get_use_coordinate_limit_forces())
{
    SimTK::State& initState = _cat.initSystem();
    // Loop over all forces in model.
    using OpenSim::CoordinateLimitForce;
    for (int iFor = 0; iFor < _cat.getForceSet().getSize(); iFor++)
    {
        CoordinateLimitForce * LF = dynamic_cast<CoordinateLimitForce *> (&_cat.updForceSet().get(iFor));
        if (LF)
        { // If it is a coordinate limit force, disable it.
            _cat.updForceSet().get(iFor).setDisabled(initState, true);
        }
    }
}

_numOptimSplinePoints = _tool.get_num_optim_spline_points();

// Prepare output
// --------------------------------------------------------------------

// Create a results directory for all the output files.
#if defined(_WIN32)
    _mkdir(_name.c_str());
#else
    mkdir(_name.c_str(), 0777);
#endif

// Serialize what we just deserialized, in the results directory, to keep
// everything in one organized place.
_tool.print(_name + "/" + _name + "_setup.xml");

// Create a log for the objective function value and the separate terms
// that go into it.
_optLog.open((_name + "/" + _name + "_log.txt").c_str(), std::ofstream::out);
_optLog << "Flippin Felines optimization log: " << _name << std::endl;
time_t rawtime; time(&rawtime);
_optLog << ctime(&rawtime);
_optLog << "Model file name: " << _tool.get_model_filename() << std::endl;

// 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]);
} 

// Set (nondimensional) parameter limits/bounds for the actuators.
SimTK::Vector lowerLimits(getNumParameters(), -1.0);
SimTK::Vector upperLimits(getNumParameters(), 1.0);
setParameterLimits(lowerLimits, upperLimits);

SimTK::Vector initialParameters()
{
    SimTK::Vector initParams(getNumParameters(), 0.0);
    if (_tool.get_initial_parameters_filename() != "")
    { // A file is specified.
        // Deserialize the initial parameters XML file.
        OpenSim::FunctionSet initFcns(_tool.get_initial_parameters_filename());
        // Write a copy of the FunctionSet to the results directory.
        initFcns.print(_name + "/" + _name + "_initial_parameters.xml");
        OpenSim::Array<std::string> initNames;
        initFcns.getNames(initNames);
        
        // This loop is set up so that the initFcns do not need to be in the same
        // order as the optimization parameters. Also, the number of initFcns
        // specified by the initial parameters file can be greater than the
        // number of actuators in the model (assuming that the initial parameters
        // file AT LEAST contains the model's actuators).
        using OpenSim::SimmSpline;
        for (int iAct = 0; iAct < _numActuators; iAct++)
        {
            // Get index of the initFcn corresponding to the actuator.
            int iFcn = initFcns.getIndex(_cat.getActuators().get(iAct).getName());
            // Get access to the spline's methods.
            SimmSpline * fcn =
                dynamic_cast<SimmSpline *>(&initFcns.get(iFcn));
            for (int iPts = 0; iPts < _numOptimSplinePoints; iPts++)
            {
                // Find the right index in the optimization parameters.
                int paramIndex = iAct * _numOptimSplinePoints + iPts;
                // Transfer y value from input to initial parameters.
                initParams[paramIndex] = fcn->getY(iPts);
            }
        }
    }
    return initParams;
}

// Increment the count of calls to this function.
_objectiveCalls++;

// Unpack parameters into the model (i.e., update spline points)
// --------------------------------------------------------------------
for (int iAct = 0; iAct < _numActuators; iAct++)
{
    // Max/min for all spline points for the i-th actuator.
    double minControl = _cat.getActuators().get(iAct).getMinControl();
    double maxControl = _cat.getActuators().get(iAct).getMaxControl();

    // Visit each point in this spline.
    for (int iPts = 0; iPts < _numOptimSplinePoints; iPts++)
    {
        int paramIndex = iAct * _numOptimSplinePoints + iPts;

        // Dimensionalize the control value.
        double dimControlValue;
        if  (parameters[paramIndex] < 0 && minControl < 0)
        { // parameter and minControl are both neg; must negate again.
            dimControlValue = -minControl * parameters[paramIndex];
        }
        else
        {
            dimControlValue = maxControl * parameters[paramIndex];
        }

        // Edit the spline with the dimensional control value.
        _splines[iAct]->setY(iPts, dimControlValue);
    }
}

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);
// --------------------------------------------------------------------

/**
 * Helper function for writing data to the log.
 * */
double processObjTerm(std::string name, double weight, double term) const
{
    if (_objectiveCalls % _outputPeriod == 0)
        _optLog << " " << name << " " << weight * term;
    return weight * term;
}
/**
* The following functions must be 'const' because objectiveFunc() is const;
* cannot call member functions within a const member function unless those
* functions are also const.
*
* These functions are also defined inline (definition is inside the class
* definition) for efficiency: the compiler will 'paste' the function body
* into the place where the function is called.
* */
double anteriorLegsDownTerm(
        double roll, double rollRate, double rollAccel) const
{
    if (_tool.get_anterior_legs_down_weight() != 0.0)
    {
        return processObjTerm("anterior_legs_down", _tool.get_anterior_legs_down_weight(),
                pow(roll - SimTK::Pi, 2) + _relw * pow(rollRate, 2) + _relw * pow(rollAccel, 2));
    }
    return 0.0;
}
double posteriorLegsDownTerm(
        double twist, double twistRate, double twistAccel) const
{
    if (_tool.get_posterior_legs_down_weight() != 0.0)
    {
        return processObjTerm("posterior_legs_down", _tool.get_posterior_legs_down_weight(),
                pow(twist - 0.0, 2) + _relw * pow(twistRate, 2) + _relw * pow(twistAccel, 2));
    }
    return 0.0;
} 

double sagittalSymmetryTerm(
        double hunch, double pitch, double pitchRate, double pitchAccel) const
{
    if (_tool.get_sagittal_symmetry_weight() != 0.0)
    {
        return processObjTerm("sagittal_symmetry", _tool.get_sagittal_symmetry_weight(),
                pow(hunch + 2 * pitch, 2) + _relw * pow(pitchRate, 2) + _relw * pow(pitchAccel, 2));
    }
    return 0.0;
}

_lastCallWasBestYet = _thisCallIsBestYet;
_thisCallIsBestYet = f <= _objectiveFcnValueBestYet;
if (_thisCallIsBestYet) _objectiveFcnValueBestYet = f;
if (_objectiveCalls % _outputPeriod == 0)
    _optLog << " objfcn " << f << " objfcn_best_yet " << _objectiveFcnValueBestYet << std::endl;
// If this is the best yet (i.e., smallest objective function value),
// save a copy of the splines.
if (_thisCallIsBestYet)
{
    for (unsigned int iFcn = 0; iFcn < _splinesBestYet.size(); iFcn++)
        _splinesBestYet[iFcn] = *_splines[iFcn];
}
// If this is worse, print out the best-yet splines and current model
// (NOTE: current model is NOT "best yet", but the idea is that it will
// be close). Also print out nondimensionalized splines so that they can
// be used directly as input for a subsequent optimization.
if (_lastCallWasBestYet && !_thisCallIsBestYet)
{
    printBestYetPrescribedControllerFunctionSet(
            _name + "_best_yet_parameters.xml");
    printBestYetPrescribedControllerFunctionSet(
            _name + "_best_yet_parameters_nomdim.xml", true);
    printModel(_name + "_best_yet.osim");
}
// Print out to the terminal/console every so often.
if (_objectiveCalls % _outputPeriod == 0)
    std::cout << "Objective call # " << _objectiveCalls << std::endl;
return 0;

// 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;