Operational-Space Controller API Tutorial
In this tutorial, we add an operational-space controller to an existing OpenSim model and use the keyboard to interact with the simulation.
Set up an example model
Download the Full Body Model from https://simtk.org/project/xml/downloads.xml?group_id=516.
Rename "FullBodyModel_SimpleArms_Hamner2010_Markers_v2_0.osim" to "FullBodyModel.osim".
Remove all actuators from the model.
Make the following adjustment at <CustomJoint name="ground_pelvis">:
<orientation_in_parent> 1.57079 0.00000000 0.00000000 </orientation_in_parent>
Start filling in your main.cpp:
Example Model
#include <OpenSim/OpenSim.h>
#include <math.h>
using namespace OpenSim;
using namespace SimTK;
#include"Controller_OP.hpp"
#include"Input.hpp"
int main()
{
try {
osimModel = Model("FullBodyModel.osim");
osimModel.setName("Body");
// Define gravity
osimModel.setGravity(Vec3(0.0, 0.0, -9.80665));
Next, we set the joint properties and define actuators at each generalized coordinates. Do not use locked or clamped joints. If you want a joint to be locked, remove it from the model. This is essential because the controller does not account for locked joints.
Joint Properties and Actuators
CoordinateSet& all_coordinates = osimModel.updCoordinateSet();
int num_coordinates = all_coordinates.getSize();
std::vector<CoordinateActuator*> myactuators;
for (int i=0; i < num_coordinates; i++ )
{
std::string coordinate_name = all_coordinates.get(i).getName();
all_coordinates.get(i).setDefaultClamped(false);
all_coordinates.get(i).setDefaultLocked(false);
std::cout << coordinate_name << all_coordinates.get(i).getDefaultValue() << std::endl;
myactuators.push_back(new CoordinateActuator(coordinate_name) );
myactuators.back()->setName("gc_act_" + coordinate_name);
osimModel.addForce( myactuators.back() );
}Get the API files
Copy the following two files into your working directory (the folder where your main.cpp is located):
Controller_OP.hpp contains the operational space framework; Input.hpp contains the keyboard callback handlers.
Integrate the Operational-Space Controller
This must be appended to the main.cpp:
Generate Controller
Vec3 pos_in_parent(0.101, -0.1, 0.001); // OP-point position relative to the parent frame
OPController *opController = new OPController(300.0, 30.0, "hand_r", pos_in_parent);
opController->setActuators(osimModel.updActuators());
opController->setName("controller_right_hand");
osimModel.addController(opController);The OP controller can be invoked simply by adding this code block. It requires the following arguments:
Argument | Description |
|---|---|
arg_kp (double) | unit-mass position gain |
arg_kv (double) | unit-mass velocity gain (damping) |
arg_parent_frame (std::string) | name of the parent frame, choose one from the .osim file <Body name= ... > |
arg_pos_in_parent (Vec3) | op-point position relative to the parent frame |
Set up the simulation
This step is standard.
Simulation
// Define the initial and final simulation times
double initialTime = 0.0;
double finalTime = 230.00;
osimModel.setUseVisualizer(true);
std::cout << "Visualizer defined" << std::endl;
// Initialize the system and get the default state
osimModel.buildSystem();Set up the keyboard callback
Keyboard
if (osimModel.hasVisualizer() )
{
OpenSim::ModelVisualizer& osim_viz = osimModel.updVisualizer();
Visualizer::InputSilo& silo = osim_viz.updInputSilo();
Controller& c = osimModel.updControllerSet().get("controller_right_hand");
OPController& opc = dynamic_cast<OPController&>(c);
osimModel.updMultibodySystem().addEventHandler(new UserInputHandler(silo, 0.1, opc)); // check every 100ms
}Finish the simulation setup
Simulation
SimTK::State& si = osimModel.initializeState();
std::cout << "System initialized" << std::endl;
osimModel.getMultibodySystem().realize(si, Stage::Velocity);
std::cout << "getMultibodySystem Done" << std::endl;
SimTK::RungeKuttaMersonIntegrator integrator(osimModel.getMultibodySystem());
integrator.setAccuracy(1.0e-3);
Manager manager(osimModel, integrator);
manager.setInitialTime(initialTime);
manager.setFinalTime(finalTime);
std::cout<<"\nIntegrating from "<<initialTime<<" to "<<finalTime<<std::endl;
manager.integrate(si);
}
catch (OpenSim::Exception ex)
{
std::cout << ex.getMessage() << std::endl;
return 1;
}
catch (std::exception ex)
{
std::cout << ex.what() << std::endl;
return 1;
}
catch (...)
{
std::cout << "UNRECOGNIZED EXCEPTION" << std::endl;
return 1;
}
std::cout << "OpenSim example completed successfully" << std::endl;
std::cout << "Press return to continue" << std::endl;
std::cin.get();
return 0;
}Comments
This concludes the tutorial. The controller is now ready to use. Experiment with the parameters, attach the operational point to different bodies, and change the gains. You can set the desired position (red sphere) using the WASDQE keys.