Overview
Here we explain loading model to ModelLoader by GryUI in general and the simulation control in DynamicsServer bycreating a scheduler.
Below is the list of steps you go through:
- Initialize CORBA
- Retrieve NamingService
- Retrieve ModelLoader and then load the model
- Retrieve DynamicsServer and configure simulation
- Retrieve OnlineViewer and load the model
- Retrieve Controller
- Simulation loop
Initialize CORBA
First, you initialize the CORBA ORB and retrieve the reference to the NamingService.
// Initializing CORBA
CORBA::ORB_var orb;
orb = CORBA::ORB_init(argc, argv);
// ROOT POA
CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
// Rtrieve the reference to the POA Manager
PortableServer::POAManager_var manager = rootPOA -> the_POAManager();
CosNaming::NamingContext_var cxT;
{
CORBA::Object_var nS = orb->resolve_initial_references("NameService");
cxT = CosNaming::NamingContext::_narrow(nS);
}
Retrieving ModelLoader
First, you prepare for the following function to retrieve a group of servers.
template
X_ptr checkCorbaServer(std::string n, CosNaming::NamingContext_var &cxt)
{
CosNaming::Name ncName;
ncName.length(1);
ncName[0].id = CORBA::string_dup(n.c_str());
ncName[0].kind = CORBA::string_dup("");
X_ptr srv = NULL;
try {
srv = X::_narrow(cxt->resolve(ncName));
} catch(const CosNaming::NamingContext::NotFound &exc) {
std::cerr << n << " not found: ";
switch(exc.why) {
case CosNaming::NamingContext::missing_node:
std::cerr << "Missing Node" << std::endl;
case CosNaming::NamingContext::not_context:
std::cerr << "Not Context" << std::endl;
break;
case CosNaming::NamingContext::not_object:
std::cerr << "Not Object" << std::endl;
break;
}
return (X_ptr)NULL;
} catch(CosNaming::NamingContext::CannotProceed &exc) {
std::cerr << "Resolve " << n << " CannotProceed" << std::endl;
} catch(CosNaming::NamingContext::AlreadyBound &exc) {
std::cerr << "Resolve " << n << " InvalidName" << std::endl;
}
return srv;
}
After getting the specified type from the NamingService, this is handled various exceptions. Retrieve ModelLoader and DynamicsSimulator using this function.
DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
dynamicsSimulatorFactory =
checkCorbaServer ("DynamicsSimulatorFactory", cxT);
if (CORBA::is_nil(dynamicsSimulatorFactory)) {
std::cerr << "DynamicsSimulatorFactory not found" << std::endl;
}
DynamicsSimulator_var dynamicsSimulator = dynamicsSimulatorFactory->create();
Here retrieve DynamicsSimulatorFactory, and then invoke DynamicsSimulator using the create method.
Next, retrieve ModelLoader.
ModelLoader_var modelLoader =
checkCorbaServer ("ModelLoader", cxT);
if (CORBA::is_nil(modelLoader)) {
std::cerr << "ModelLoader not found" << std::endl;
}
Load model for the acquired the ModelLoader and the DynamicsSimulator.
CharacterInfo_ptr cInfo = modelLoader->loadURL(ROBOT_URL);
dynamicsSimulator->registerCharacter("robot", cInfo);
This makes the ModelLoader load the model in the 1st line and registers it as the character "robot" in the 2nd line. Likewise, it registers the floor.
cInfo = modelLoader->loadURL(FLOOR_URL);
dynamicsSimulator->registerCharacter("floor", cInfo);
DynamicsSimulator
You initialize the DynamicsSimulator.
dynamicsSimulator->init(0.002, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);
The time of every step of the simulation, the difference method, the activation/deactivation of the sensor are set in the init method. The DynamicsSimulator::EULER that uses the Euler method and the DynamicsSimulator::RUNGE_KUTTA that uses the Runge-Kutta method in the difference method. There are the DynamicsSimulator::ENABLE_SENSOR for activation and the DynamicsSimulator::DISABLE_SENSOR for deactivation in the sensor.
Next, set the gravity vector.
DblSequence3 gVector;
gVector.length(3);
gVector[0] = gVector[1] = 0;
gVector[2] = 9.8;
dynamicsSimulator->setGVector(gVector);
In this case, the gravitational acceleration will be 9.8ms^-2 downward.
Also, set the mode for driving a joint.
dynamicsSimulator->setCharacterAllJointModes("robot", DynamicsSimulator::TORQUE_MODE);
All joints of the character "Robot" is set to the torque mode by this code.
There is also a high gain mode DynamicsSimulator::HIGH_GAIN_MODE other than the above used DynamicsSimulator::TORQUE_MODE in the mode for joints.
Next, decide an initial posture of the robot.
Please set it by using DynamicsSimulator::setCharacterAllLinkData etc because you can use it.
Calculate the forward kinematics once to reflect the posture after setting.
dynamicsSimulator->calcWorldForwardKinematics();
Next, register the collision detection pairs. The collision detection pairs registered here will be done.
This pairs the robot with the floor.
DblSequence6 sc, dc;
sc.length(0);
dc.length(0);
dynamicsSimulator->registerCollisionCheckPair
("robot",
"",
"floor",
"",
0.5,
0.5,
sc,
dc);
Finally, invoke the initSimulation method.
dynamicsSimulator->initSimulation();
OnlineViewer
Retrive OnlineViewer.
OnlineViewer_var onlineViewer =
checkCorbaServer ("OnlineViewer", cxT);
if (CORBA::is_nil(onlineViewer)) {
std::cerr << "OnlineViewer not found" << std::endl;
}
In addition, the model is loaded by OnlineViewer.
onlineViewer->load("robot", ROBOT_URL);
onlineViewer->load("floor", FLOOR_URL);
Controller
ControllerFactory is acquired by using each Controller name.
ControllerFactory_var controllerFactory;
controllerFactory =
checkCorbaServer ("PDcontroller", cxT);
if (CORBA::is_nil(controllerFactory)) {
std::cerr << "ControllerFactory not found" << std::endl;
}
In this case, ControllerFactory of Controller named PDcontroller is acquired.
Controller is acquired from "Factory". In that case, you specify a controller for each character.
Controller_var controller = controllerFactory->create("robot");
Also, DynamicsSimulator is set in Controller.
controller->setDynamicsSimulator(dynamicsSimulator);
You start it at the end.
controller->start();
Simulation loop
This is a turned loop when an actual simulation is executed. This simulation loop consists of as follow.
- Update OnlineViewer
- Invoke Controller::input
- Invoke Controller::control
- Invoke DynamicsSimulator::stepSimulation
- Invoke Controller::output
The code is described as follows:
WorldState_var state;
while (1) {
// Retrive WorldState and pass it to OnlineViewer
dynamicsSimulator->getWorldState(state);
onlineViewer->update(state);
// Invoke input
controller->input();
// Invoke control
controller->control();
// Invoke stepSimulation and proceed the simulation
dynamicsSimulator->stepSimulation();
// Invoke output
controller->output();
}
Additional information
In above simulation loop, a normal vector by the collision is displayed, and a normal vector can be also edited and used for other purposes.
This can be realized by editing collisions in WorldState as follows:
dynamicsSimulator->getWorldState(state);
state->collisions.length(1);
state->collisions[0].pair.charName1 = CORBA::string_dup("robot");
state->collisions[0].pair.charName2 = CORBA::string_dup("floor");
state->collisions[0].pair.linkName1 = CORBA::string_dup("RLEG_ANKLE_R");
state->collisions[0].pair.linkName2 = CORBA::string_dup("WAIST");
state->collisions[0].points.length(1);
state->collisions[0].points[0].position[0] = 0.0;
state->collisions[0].points[0].position[1] = 0.0;
state->collisions[0].points[0].position[2] = 0.0;
state->collisions[0].points[0].normal[0] = 0.0;
state->collisions[0].points[0].normal[0] = 0.0;
state->collisions[0].points[0].normal[0] = 0.5;
onlineViewer->update(state);
This code is set to draw a normal vector from (0.0,0.0,0.0) to (0.0,0.0,0.5) between the character name "robot" and "floor".