Overview
This page explains some of the specific code related to this project. Some of the code is similar to the example given in the Creating an Optimization example that is distributed with OpenSim. Please consult that page for support on generally setting up an optimization.
Global Variables
The global variables are modified from the optimization example.
// Global variables
int stepCount = 0;
double initialTime = 0.0;
double finalTime = 1.0;
double bestSoFar = Infinity;
double forceThreshold = 1.0;
double settleTime = 0.2;
ofstream optLog("optLog.txt", ofstream::out);
ofstream bestSoFarLog("bestSoFarLog.txt", ofstream::out);
ofstream verboseLog("verboseLog.txt", ofstream::out);
ofstream debugLog("debugLog.txt", ofstream::out);
std::clock_t optimizerStartTime;
A forceThreshold is added for the event handler. This is the total amount of vertical force for the contact points when the event handler will trigger. A settleTime is added so that the feet can come off initially without the event handler terminating the simulation early. Four files are also added to help with output.
Main Function
The main function handles reading in the model file, adding all of the controllers, and calling on the optimizer. The first part of the code reads in the model file and initializes the controller parameters.
int main()
{
try {
std::clock_t startTime = std::clock();
// Ensures all components are printed out to the model file.
Object::setSerializeAllDefaults(true);
// Create a new OpenSim model from file
Model osimModel("jumper10dof24musc.osim");
// The number of parameters is the number of actuators
const Set<Actuator> &actuatorSet = osimModel.getActuators();
int numActuators = actuatorSet.getSize();
optLog << "numActuators = " << numActuators << endl;
int numParameters = numActuators;
/* Define initial values for controllerParameters. Each controller has two parameters, an
initial time, and a duration for bang-bang control. There are as many controls as
there are actuators because we assume symmetry (but each controller has two parameters).
controllerParameters = [ti_0 duration_0 ti_1 duration_1 ... ]' */
// initialize controller parameters
Vector controllerParameters(numParameters, 0.05);
// initialize initial time for each excitation
controllerParameters[0] = 0.107829; //hamstrings
controllerParameters[2] = 0.000694389; //bifmesh
controllerParameters[4] = 0.296862; //glut_max
controllerParameters[6] = 0.0533367; //iliopsoas
controllerParameters[8] = 0.166411; //rect_fem
controllerParameters[10] = 0.277515; //vasti
controllerParameters[12] = 0.34415; //gastroc
controllerParameters[14] = 0.315352; //soleus
controllerParameters[16] = 0.0857547; //tib_ant
controllerParameters[18] = 0.149619; //ercspn
controllerParameters[20] = 0.468751; //intobl
controllerParameters[22] = 0.455196; //extobl
// initialize durations
controllerParameters[1] = 0.429037; //hamstrings
controllerParameters[3] = 0.12063; //bifemsh
controllerParameters[5] = 0.392451; //glut_max
controllerParameters[7] = 0.0303019; //iliopsoas
controllerParameters[9] = 0.370407; //rect_fem
controllerParameters[11] = 0.3; //vasti
controllerParameters[13] = 0.343607; //gastroc
controllerParameters[15] = 0.350789; //soleus
controllerParameters[17] = 0.0276857; //tib_ant
controllerParameters[19] = 0.243365; //ercspn
controllerParameters[21] = 0.160011; //intobl
controllerParameters[23] = 0.151908; //extobl
The line to setSerializeAllDefaults(true) ensures that all parts of the model are printed out, but is not necessary after OpenSim 3.1. The vector of controllerParameters represents the initial time of the "bang" of the control and the duration of the "bang". Thus, the even indices correspond to initial time for each actuator, and the odd indices for the durations. The controllerParameters are initialized in this block of code and can be changed by the user in this .cpp file.
The next part of the code adds controllers to each of the actuators, enforcing symmetry.
// Add prescribed controllers to each muscle. Need to only loop numActuators/2 times since we are enforcing symmetry.
// It is assumed that all actuators are listed for one side of the model first, then the other side, in the same order.
for(int i=0; i < numActuators/2; i++) {
// make a piecewise constant function for both sides
PiecewiseConstantFunction bangBangControl;
bangBangControl.addPoint(initialTime,0);
bangBangControl.addPoint(controllerParameters[2*i],1);
bangBangControl.addPoint(controllerParameters[2*i]+controllerParameters[2*i+1],0);
// add controller to right side
PrescribedController *actuatorController_r = new PrescribedController();
Set<Actuator> actuator_r;
actuator_r.insert(0,osimModel.getActuators().get(i));
actuatorController_r->setName(actuatorSet.get(i).getName());
actuatorController_r->setActuators(*actuator_r.clone());
actuatorController_r->prescribeControlForActuator(osimModel.getActuators().get(i).getName(), bangBangControl.clone());
osimModel.addController(actuatorController_r);
// add controller to left side
PrescribedController *actuatorController_l = new PrescribedController();
Set<Actuator> actuator_l;
actuator_l.insert(0,osimModel.getActuators().get(i + numActuators/2));
actuatorController_l->setName(actuatorSet.get(i + numActuators/2).getName());
actuatorController_l->setActuators(*actuator_l.clone());
actuatorController_l->prescribeControlForActuator(osimModel.getActuators().get(i + numActuators/2).getName(), bangBangControl.clone());
osimModel.addController(actuatorController_l);
}
The order of the actuators are assumed to be in the same order for the left and right sides, with all of the actuators on one side listed first, then the other side. Symmetry is ensured by assigning the same PiecewiseConstantFunction for the controller added to each side.
The last part is to set the settings of the Optimizer and OptimizerSystem.
// Create the OptimizationSystem. Initialize the objective function value "f".
JumpingOptimizationSystem sys(numParameters, osimModel);
Real f = NaN;
// Set lower and upper bounds.
Vector lower_bounds(numParameters, initialTime);
Vector upper_bounds(numParameters, finalTime);
// Limit the duration of the "bang" to be at least a certain value to decrease search space.
for (int i = 1; i<numParameters; i+=2) {
lower_bounds[i] = 0.0001;
}
sys.setParameterLimits( lower_bounds, upper_bounds );
// Create an optimizer. Pass in our OptimizerSystem
// and the name of the optimization algorithm.
optLog << "using LBFGSB" << endl; Optimizer opt(sys, SimTK::LBFGSB); //LBFGSB was found to be beter for this problem
//optLog << "using IPOPT" << endl; Optimizer opt(sys, InteriorPoint);
// Specify settings for the optimizer
opt.setConvergenceTolerance(0.01);
opt.useNumericalGradient(true);
opt.setMaxIterations(1000);
opt.setLimitedMemoryHistory(500);
//opt.setDiagnosticsLevel(4); // Lowest level that gives outer loop information for IPOPT
// Optimize it!
optLog << "Optimizing!" << endl;
optLog << "Initial controllerParameters: " << controllerParameters << endl;
optLog << "lower_bounds: " << lower_bounds << endl;
optLog << "upper_bounds: " << upper_bounds << endl;
f = opt.optimize(controllerParameters);
The lower_bounds for the durations were set to be small, but not 0, in order to decrease the search space of arbitrarily small excitation patterns that will not affect the force output in the end.
Event Handler
An event handler is used to stop the simulation once the model leaves the ground (i.e. the vertical force at the contact points reaches forceThreshold). At this point, the final height is known by the ballistic trajectory, so we can save time by not simulating time in the air.
//=============================================================================
// EVENT HANDLER: TerminateSimulation
// Triggered when the GRF normal falls below some threshold
//=============================================================================
class TerminateSimulation : public SimTK::TriggeredEventHandler {
public:
// CONSTRUCTOR
TerminateSimulation(const Model& m, double threshold) : TriggeredEventHandler(Stage::Dynamics),
_model(m), _forceThreshold(threshold)
{
getTriggerInfo().setTriggerOnFallingSignTransition(true);
}
// WITNESS FUNCTION
SimTK::Real getValue(const SimTK::State& s) const
{
// keep returning a positive value while the contact settles out
double simTime = s.getTime();
if (simTime < settleTime) {
return 100.0;
}
// total vertical force applied to both feet
Array<double> force_foot_r = _model.getForceSet().get("foot_r").getRecordValues(s);
Array<double> force_foot_l = _model.getForceSet().get("foot_l").getRecordValues(s);
double netGRFVertical = -force_foot_r[1] - force_foot_l[1];
return netGRFVertical - _forceThreshold;
}
// EVENT HANDLER FUNCTION
void handleEvent (SimTK::State& s, SimTK::Real accuracy, bool& terminate) const
{
//cout << "terminated because vertical force passed threshold" << endl; //debug line
terminate = true;
}
private:
const Model& _model;
double _forceThreshold;
};
The class TerminateSimulation inherits properties from TriggeredEventHandler. The constructor needs to know what stage it needs to reach to interpret the witness function, which is dynamics for this case since we are interested in forces. It also needs to know that it will trigger when the sign change is falling (i.e. goes from positive to negative). The witness function returns a value at each integration time step. We see that before settleTime is reached, a positive number is always returned. After we reach the settleTime, we retrieve the forces on both feet and return back the sum of the vertical ground reaction force minus the forceThreshold. The last part is to fill in handleEvent, which tells the event handler what to do when the event is triggered.
Objective Function
The objective function first updates the control values with the newControllerParameters.
/* Objective Function. */
int objectiveFunc( const Vector &newControllerParameters, bool new_coefficients, Real& f ) const {
// Grab actuator and controller sets
const Set<Actuator> &actuatorSet = osimModel.getActuators();
int numActuators = actuatorSet.getSize();
const ControllerSet &controllerSet = osimModel.getControllerSet();
int numControllers = controllerSet.getSize();
// Update the control values
for (int i = 0; i < numControllers/2; i++) {
// define a piecewise constant function for both sides
PiecewiseConstantFunction bangBangControl;
bangBangControl.addPoint(initialTime, 0);
bangBangControl.addPoint(newControllerParameters[2*i],1);
bangBangControl.addPoint(newControllerParameters[2*i]+newControllerParameters[2*i+1],0);
// Update the right side
PrescribedController* controller_r = dynamic_cast<PrescribedController*>( &controllerSet.get(2*i) );
controller_r->prescribeControlForActuator(actuatorSet.get(i).getName(), bangBangControl.clone());
// Update the left side
PrescribedController* controller_l = dynamic_cast<PrescribedController*>( &controllerSet.get(2*i+1) );
controller_l->prescribeControlForActuator(actuatorSet.get(i + numActuators/2).getName(), bangBangControl.clone());
}
A new PiecewiseConstantFunction is defined using the newControllerParameters passed in by the optimizer. These are then used to replace the prescribed controller for each of the controllers in the model, again enforcing symmetry.
The next step is to initialize the system. We see here, though, that initSystem() cannot be used because the event handler must be added between the steps of buildSystem() and initializeState().
// Initialize the system. initSystem() cannot be used here because adding the event handler
// must be done between buildSystem() and initializeState().
osimModel.buildSystem();
TerminateSimulation *terminate = new TerminateSimulation(osimModel, forceThreshold);
osimModel.updMultibodySystem().addEventHandler(terminate);
State &osimState = osimModel.initializeState();
Similar steps as the optimization example are needed to initialize activations, equilibrate the muscles, and setup and run the forward integration.
// Set the initial muscle activations
const Set<Muscle> &muscleSet = osimModel.getMuscles();
for(int i=0; i< muscleSet.getSize(); i++ ){
muscleSet[i].setActivation(osimState, 0.05);
}
// Make sure the muscles states are in equilibrium
osimModel.equilibrateMuscles(osimState);
// Create the integrator for the simulation.
RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-6); //1.0e-6 seems to converge faster (both objFunc calls and time) than 1.0e-5
// Create a manager to run the simulation. Can change manager options to save run time and memory or print more information
Manager manager(osimModel, integrator);
//manager.setWriteToStorage(false);
manager.setPerformAnalyses(false);
// Integrate from initial time to final time and integrate
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
manager.integrate(osimState);
The last key part is evaluating the objective function to pass to the Optimizer.
/* Calculate the scalar quantity we want to minimize or maximize.
* In this case, we’re maximizing the height of the COM of the jumper
* so to maximize, calculate (position + velocity^2)/(2g) when simulation ends.
* Then take the negative of that quantity (since optimizers minimize).
*/
osimModel.getMultibodySystem().realize(osimState, Stage::Velocity);
Vec3 COM_position = osimModel.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterLocationInGround(osimState);
Vec3 COM_velocity = osimModel.getMultibodySystem().getMatterSubsystem().calcSystemMassCenterVelocityInGround(osimState);
double g = -osimModel.getGravity()[1];
double maxHeight = COM_position[1] + pow(COM_velocity[1], 2.0)/(2.0*g);
f = -maxHeight;
We must first realize the system to velocity since we need position and velocity to evaluate the objection function. We get the position and velocity of the system's center of mass (the ground has a mass of 0 in the model) by going to the MultibodySystem. Gravity is found by calling on the model. As with the Arm26 Optimization Example, the negative of the value is taken since optimizers minimize objective functions.