Operational-Space Controller API Tutorial
- Former user (Deleted)
- Thomas Uchida
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:
#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.
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:
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.
// 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
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
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.
OpenSim is supported by the Mobilize Center , an NIH Biomedical Technology Resource Center (grant P41 EB027060); the Restore Center , an NIH-funded Medical Rehabilitation Research Resource Network Center (grant P2C HD101913); and the Wu Tsai Human Performance Alliance through the Joe and Clara Tsai Foundation. See the People page for a list of the many people who have contributed to the OpenSim project over the years. ©2010-2024 OpenSim. All rights reserved.