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:
- load the model
- Initialize CORBA
- Retrieve NamingService
- Retrieve OnlineViewer and load the model
- Retrieve DynamicsServer and configure simulation
- Retrieve Controller
- Simulation loop
Load the Model
First, load a model file using ModelLoader.
Please specify the path to the model file of a floor and a robot as Model [0] and model [1].
BodyInfo_var floor = loadBodyInfo(Model[0].c_str(), argc, argv);
BodyInfo_var body = loadBodyInfo(Model[1].c_str(), argc, argv);
Initialize CORBA
Next, you initialize the CORBA ORB and retrieve the reference to the NamingService.
CORBA::ORB_var orb;
orb = CORBA::ORB_init(argc, argv);
CORBA::Object_var poaObj = orb -> resolve_initial_references("RootPOA");
PortableServer::POA_var rootPOA = PortableServer::POA::_narrow(poaObj);
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);
OnlineViewer
Retrieve OnlineViewer.
OnlineViewer_var olv = getOnlineViewer(argc, argv);
In addition, the model is loaded by OnlineViewer.
olv->load(floor->name(), Model[0].c_str());
olv->load(body->name(), Model[1].c_str());
olv->clearLog();
DynamicsSimulator
You prepare for the following function to retrieve a group of servers.
template <typename X, typename X_ptr>
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 DynamicsSimulatorFactory using this function.
DynamicsSimulatorFactory_var dynamicsSimulatorFactory;
dynamicsSimulatorFactory =
checkCorbaServer <DynamicsSimulatorFactory, DynamicsSimulatorFactory_var> ("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, you initialize the DynamicsSimulator.
dynamicsSimulator->registerCharacter(floor->name(), floor);
dynamicsSimulator->registerCharacter(body->name(), body);
dynamicsSimulator->init(timeStep, DynamicsSimulator::RUNGE_KUTTA, DynamicsSimulator::ENABLE_SENSOR);
The model of a floor and a robot is registered into DynamicsSimulator.
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 g;
g.length(3);
g[0] = 0.0;
g[1] = 0.0;
g[2] = world_gravity;
dynamicsSimulator->setGVector(g);
Next, decide an initial posture of the robot.
Vector3 waist_p;
Matrix33 waist_R;
waist_p = 0, 0, 0.7135;
waist_R = tvmet::identity<Matrix33>();
DblSequence trans;
trans.length(12);
for(int i=0; i<3; i++) trans[i] = waist_p(i);
for(int i=0; i<3; i++){
for(int j=0; j<3; j++) trans[3+3*i+j] = waist_R(i,j);
}
dynamicsSimulator->setCharacterLinkData( body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, trans );
decide an initial angle of the joint.
DblSequence angle;
angle.length(29);
angle[0] = 0.0; angle[1] = -0.0360373; angle[2] = 0.0; angle[3] = 0.0785047;
angle[4] = -0.0424675; angle[5] = 0.0; angle[6] = 0.174533; angle[7] = -0.00349066;
angle[8] = 0.0; angle[9] = -1.5708; angle[10] = 0.0; angle[11] = 0.0;
angle[12] = 0.0; angle[13] = 0.0; angle[14] = -0.0360373; angle[15] = 0.0;
angle[16] = 0.0785047; angle[17] = -0.0424675; angle[18] = 0.0; angle[19] = 0.174533;
angle[20] = -0.00349066;angle[21] = 0.0; angle[22] = -1.5708; angle[23] = 0.0;
angle[24] = 0.0; angle[25] = 0.0; angle[26] = 0.0; angle[27] = 0.0;
angle[28] = 0.0;
dynamicsSimulator->setCharacterAllLinkData( body->name(), DynamicsSimulator::JOINT_VALUE, angle );
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 K, C;
K.length(0);
C.length(0);
dynamicsSimulator->registerCollisionCheckPair(floor->name(),"", body->name() ,"",
statFric,slipFric,K,C,culling_thresh);
Finally, invoke the initSimulation method.
dynamicsSimulator->initSimulation();
Controller
Controller is acquired by using each Controller name.
Controller_var controller;
controller = checkCorbaServer <Controller, Controller_var> ("SamplePDController", cxt);
if (CORBA::is_nil(controller)) {
std::cerr << "Controller not found" << std::endl;
}
In this case, Controller named PDcontroller is acquired.
Initialize the Controller.
controller->setModelName(body->name());
controller->setDynamicsSimulator(dynamicsSimulator);
controller->initialize();
controller->setTimeStep(controlTimeStep);
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.
- Invoke Controller::input（Robot data is inputted into Controller from DynamicsSimulator. ）
- Invoke Controller::control（Calculation of the output value of Controller ）
- Invoke DynamicsSimulator::stepSimulation (One step of a simulation）
- Invoke DynamicsSimulator::getWorldState (Read-out of a simulation result )
- Update OnlineViewer （update OnlineViewer)
- Invoke Controller::output (A control value is outputted to DynamicsSimulator from Controller. ）
The code is described as follows:
bool control=false;
if(controlTime <= time){
control=true;
j++;
}
if(control)
controller->input();
i++;
time = timeStep * i;
controlTime = controlTimeStep * j;
if(control)
controller->control();
// ================== simulate one step ==============
dynamicsSimulator->stepSimulation();
// ================== viewer update ====================
try {
dynamicsSimulator -> getWorldState( state );
olv->update( state );
} catch (CORBA::SystemException& ex) {
return 1;
}
// ===================== get robot status ===================
DblSequence_var waist_pR; // position and attitude
DblSequence_var waist_vw; // linear and angular velocities
dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_TRANSFORM, waist_pR);
dynamicsSimulator->getCharacterLinkData(body->name(), "WAIST", DynamicsSimulator::ABS_VELOCITY, waist_vw);
// ================== log data save =====================
log_file << time << " ";
log_file << waist_vw[2] << " ";
log_file << endl;
if(control)
controller->output();
if( time > EndTime ) break;
Execution
- Start GrxUI.
- Execut OpenHRP/sample/controller/SamplePD/SamplePD.sh (.bat) from a command line.
- Execut OpenHRP/sample/example/scheduler/scheduler.sh (.bat) from a command line.
In the case of Windows, please add (OpenHRP3)/bin to PATH.
