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:

  1. Initialize CORBA
  2. Retrieve NamingService
  3. Retrieve ModelLoader and then load the model
  4. Retrieve DynamicsServer and configure simulation
  5. Retrieve OnlineViewer and load the model
  6. Retrieve Controller
  7. 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.

  1. Update OnlineViewer
  2. Invoke Controller::input
  3. Invoke Controller::control
  4. Invoke DynamicsSimulator::stepSimulation
  5. 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".