Creating a Controller Part Two

The steps covered in part two are:

Writing the main()

To run a forward dynamics simulation using our controller, we can write a main program (as in Chapter 2). Our main program below initializes the model, attaches a controller to the model, and runs a forward dynamics simulation.

int main()
{
  try {
    // Create an OpenSim model from the model file provided. 
    Model osimModel( "tugOfWar_model_ThelenOnly.osim" );

Creating the controller and attaching it to the model

Next in our main program, we create an instance of the TugOfWarPDController, passing it the model we read in as well as values we choose for the position and velocity gains:

  // Create the controller.
  TugOfWarPDController *pdController = new
  TugOfWarPDController( osimModel, kp, kv ); 
  
  // Add the controller to the Model.
  osimModel.addController( pdController );

Initializing the system

Next, we set the initial states of the system, which include the position and speed values for all the coordinates in the model, as well as the muscle activations and fiber lengths:

  // Initialize the system and get the state representing the
  // system.
  SimTK::State& si = osimModel.initSystem(); 

  // Define non-zero (defaults are 0) states for the free joint.
  CoordinateSet& modelCoordinateSet = osimModel.updCoordinateSet();

  // Get the z translation coordinate.
  Coordinate& zCoord = modelCoordinateSet.get( "blockToGround_zTranslation" );

  // Set z translation speed value.
  zCoord.setSpeedValue( si, 0.15 * Pi ); 
 
  // Define the initial muscle states.
  const Set<Actuator>& actuatorSet = osimModel.getActuators();
  Muscle* muscle1 = dynamic_cast<Muscle*>( &actuatorSet.get(0) );
  Muscle* muscle2 = dynamic_cast<Muscle*>( &actuatorSet.get(1) );
  
  muscle1->setDefaultActivation( 0.01 ); // muscle1 activation
  muscle1->setDefaultFiberLength( 0.2 ); // muscle1 fiber length
  muscle2->setDefaultActivation( 0.01 ); // muscle2 activation
  muscle2->setDefaultFiberLength( 0.2 ); // muscle2 fiber length 

  // Compute initial conditions for muscles.
  osimModel.equilibrateMuscles( si );

Creating the integrator

We create the integrator for the simulation in order to perform the numerical integration of the system equations of motion during the forward dynamics simulation.

  // Create the integrator and manager for the simulation.
  SimTK::RungeKuttaMersonIntegrator integrator( osimModel.getMultibodySystem() );
  integrator.setAccuracy( 1.0e-4 );
  integrator.setAbsoluteTolerance( 1.0e-4 );
  Manager manager( osimModel, osimModel.getMultibodySystem(),integrator );

Running the simulation

We run the simulation by setting the initial and final times for the integrator and then instructing the manager to integrate starting from the initial state of the system.

  // Integrate from initial time to final time.
  manager.setInitialTime( initialTime );
  manager.setFinalTime( finalTime );
  std::cout << "\n\nIntegrating from " << initialTime << " to " << finalTime << std::endl;
  manager.integrate( si );

At this point, you can run the main program. We can then visualize the result of the forward dynamics simulation to see how well the controller was able to make the model follow the desired trajectory. The figure below shows the desired trajectory (magenta) and the actual trajectory (dark blue) taken by the model during the simulation. The vertical axis is the position of the block (z-coordinate) and the horizontal axis is time (in seconds):

Next: Creating an Optimization

Previous: Creating a Controller Part One

Home: Creating a Controller | Adding New Functionality | Developer's Guide