2
* Robot3D Physical realistic 3D simulator for robots
4
* @license GNU Lesser General Public License
7
* @author Anne C. van Rossum
15
#include <dtUtil/log.h>
16
#include <dtDAL/librarymanager.h>
19
#include <srCore/export.h>
20
#include <srCore/simulationEntryPoint.h>
21
#include <srCore/robotCreator.h>
22
#include <srCore/robot/robotActorBase.h>
23
#include <srCore/loadComponents.h>
25
/************************************************************************************************
27
***********************************************************************************************/
29
#define TIME_SCALE 0.1 //1
30
#define GRAVITY 200 //n*(5cm)/sĀ²
32
/************************************************************************************************
33
* Default strings / names / paths
34
***********************************************************************************************/
36
std::string DEFAULT_COMPONENT_PATH = "../data/components";
37
std::string DEFAULT_STAGEPROJECT_PATH = "../data/StageProject";
39
/************************************************************************************************
41
***********************************************************************************************/
44
using namespace srCore;
46
/************************************************************************************************
48
***********************************************************************************************/
51
* This function is obligated by the Delta3D library. It makes it possible to dynamically load the
54
extern "C" ROBOT_EXPORT dtGame::GameEntryPoint* CreateGameEntryPoint() {
55
return new SimulationEntryPoint();
59
* This function is obligated by the Delta3D library. It makes it possible to dynamically unload the
62
extern "C" ROBOT_EXPORT void DestroyGameEntryPoint(dtGame::GameEntryPoint* entryPoint) {
66
/************************************************************************************************
67
* Implementation of SimulationEntryPoint
68
***********************************************************************************************/
71
* The constructor of the Robot3D simulator. It sets the default log level to LOG_INFO and
72
* displays a welcome message.
74
SimulationEntryPoint::SimulationEntryPoint() {
75
dtUtil::Log::GetInstance().SetLogLevel(dtUtil::Log::LOG_INFO);
76
LOG_ALWAYS("Welcome to the Robot3D simulator");
78
osg::DisplaySettings::instance()->setNumMultiSamples(4);
82
* Destroy the entire Robot3D simulator.
84
* @todo Check what can be deallocated...
86
SimulationEntryPoint::~SimulationEntryPoint() {
87
LOG_INFO("Unload everything...");
92
* The start up routine. It loads a predefined XML configuration file and initiates the simulator
93
* with the data found in it. It is not the idea that this function needs to know the different types
94
* of robot, sensor, and actuator classes that can be defined. This needs to be registered by the
97
void SimulationEntryPoint::OnStartup(dtGame::GameApplication &app)
99
// app.GetGameManager()->DebugStatisticsTurnOn(true,true,1,true,"gameManagerDebugInfo.txt");
100
bool withRobotControlEnabled = true;
101
bool withGUI = false;
102
bool freeCamState = true;
106
std::ostringstream msg; msg.clear(); msg.str("");
107
dtUtil::Log::GetInstance().SetLogLevel(dtUtil::Log::LOG_INFO);
108
LOG_INFO("Beginning of Robot3D simulator startup");
110
app.GetWindow()->SetWindowTitle("Robot3D Simulation");
111
app.GetWindow()->ShowCursor(false);
113
// Set stage project path
114
std::string stageprojectPath=app.GetConfigPropertyValue("StageProject", DEFAULT_STAGEPROJECT_PATH);
115
SimulationUtils::setStageProjectPath(stageprojectPath);
117
msg.clear(); msg.str("");
118
msg << "Set StageProject path to: " << SimulationUtils::getStageProjectPath();
122
// Register new message types
123
SimulationMessageType::RegisterMessageTypes( app.GetGameManager()->GetMessageFactory() );
125
LOG_WARNING("Load actors explicitly from libRobot3D.so library");
126
dtDAL::LibraryManager::GetInstance().LoadActorRegistry("Robot3D");
132
dtAudio::AudioManager::Instantiate();
133
dtAudio::AudioManager::GetInstance().Config();
136
std::string file = srCore::SimulationUtils::getSceneFile();
138
file = app.GetConfigPropertyValue("ScenarioFile", "../data/scenarios/scene.xml");
139
srCore::SimulationUtils::setSceneFile(file);
142
msg.clear(); msg.str("");
143
msg << "Reading \"scene.xml\" file: " << srCore::SimulationUtils::getSceneFile();
145
} catch(std::runtime_error error) {
146
LOG_ERROR("Configuration file does not exist!");
150
appConfig.readConfigFile(srCore::SimulationUtils::getSceneFile());
152
msg.clear(); msg.str("");
153
msg << "Map: " << appConfig.getMap();
155
msg.clear(); msg.str("");
156
msg << "With Shadows: " << appConfig.getWithShadows();
158
msg.clear(); msg.str("");
159
msg << "With Detailed Models: " << appConfig.getWithDetailedModels();
162
msg.clear(); msg.str("");
163
msg << "Load robots...";
166
for (unsigned int i=0; i< appConfig.getRobots().size(); i++)
168
msg.clear(); msg.str("");
169
msg << "Named " << appConfig.getRobots()[i].name << " with type " << appConfig.getRobots()[i].type;
170
msg << " at position: [" << appConfig.getRobots()[i].pos.x() << "," <<
171
appConfig.getRobots()[i].pos.y() << "," << appConfig.getRobots()[i].pos.z() << "]";
172
msg << " oriented as: [" << appConfig.getRobots()[i].quat << "] " << " with joint angle: " << appConfig.getRobots()[i].jointAngles[0];
177
msg.clear(); msg.str("");
178
msg << "Load controllers...";
181
for (unsigned int i=0; i< appConfig.getControllers().size(); i++)
183
if (appConfig.getControllers().at(i).name == NULL) {
184
LOG_WARNING("There seems to be something wrong with the vector with ControllerInitData");
187
msg.clear(); msg.str("");
188
msg << "A " << appConfig.getControllers()[i].type << " controller";
189
if (strcmp(appConfig.getControllers()[i].name, "")) {
190
msg << " named \"" << appConfig.getControllers()[i].name << "\"";
192
if (strncmp(appConfig.getControllers()[i].type, "Input", strlen("Input")) == 0)
194
withRobotControlEnabled = atof(appConfig.getControllers()[i].parameter);
195
msg <<" withRobotControlEnabled="<< withRobotControlEnabled;
199
if (strncmp(appConfig.getControllers()[i].type, "GUI", strlen("GUI")) == 0)
202
if (strncmp(appConfig.getControllers()[i].type, "FreeCam", strlen("FreeCam")) == 0)
204
robotFocus = atoi(appConfig.getControllers()[i].robot);
205
camPos = appConfig.getControllers()[i].pos;
207
msg << " at position: [" << camPos.x() << "," << camPos.y() << "," << camPos.z() << "]";
211
else if (strncmp(appConfig.getControllers()[i].type, "FollowCam", strlen("FollowCam")) == 0)
213
robotFocus = atoi(appConfig.getControllers()[i].robot);
214
camPos = appConfig.getControllers()[i].pos;
216
msg << " at position: [" << camPos.x() << "," << camPos.y() << "," << camPos.z() << "]";
218
freeCamState = false;
221
if (strcmp(appConfig.getControllers()[i].robot, ""))
222
msg << " controlling robot \"" << appConfig.getControllers()[i].robot << "\" ";
227
SimulationUtils::setDetailedGraphics(appConfig.getWithDetailedModels());
228
SimulationUtils::setWithShadows(appConfig.getWithShadows());
229
SimulationUtils::setCollisionGeometryDetails(appConfig.getCollisionGeometryDetails());
230
SimulationUtils::setConnectorsAsJoints(appConfig.getConnectorsAsJoints());
231
SimulationUtils::setWithWheels(appConfig.getRobotsWithWheels());
233
//Load Map and apply Light
234
LOG_INFO("Set context, load all information from StageProject directory");
235
dtDAL::Project::GetInstance().SetContext(SimulationUtils::getStageProjectPath());
237
//Set log level temporary to WARNING to get less log messages
238
dtUtil::Log::LogMessageType lmt = dtUtil::Log::GetInstance().GetLogLevel();
239
if (lmt < dtUtil::Log::LOG_WARNING)
240
dtUtil::Log::GetInstance().SetLogLevel(dtUtil::Log::LOG_WARNING);
241
dtDAL::Map &map = dtDAL::Project::GetInstance().GetMap(appConfig.getMap());
242
dtUtil::Log::GetInstance().SetLogLevel(lmt);
244
dtDAL::Project::GetInstance().LoadMapIntoScene(map, app.GetGameManager()->GetScene(), false);
245
LOG_INFO("Map loaded");
248
app.GetView(0)->GetDatabasePager()->SetTargetFrameRate(60); //20
250
SetPhysicalParameters(app);
252
//----------------------Components----------------------------------------------------------------------------------------
254
LOG_INFO("Creating components");
256
//Add DefaultMessageProcessor
257
dtGame::DefaultMessageProcessor *dmp = new dtGame::DefaultMessageProcessor("DefaultMessageProcessor");
258
app.GetGameManager()->AddComponent(*dmp,dtGame::GameManager::ComponentPriority::HIGHEST);
260
//Add Arena Component - doing all the important stuff
261
dtCore::RefPtr<SimulationArenaComponent> arenaComp = new SimulationArenaComponent(SimulationArenaComponent::DEFAULT_NAME);
262
app.GetGameManager()->AddComponent(*arenaComp, dtGame::GameManager::ComponentPriority::LOWEST);
264
arenaComp->createRobotsFromSceneFile(appConfig);
266
//Add all GUI elements
269
LoadGUIComponents(app);
272
//Add Camera Component
273
dtCore::RefPtr<SimulationCameraController> cameraContr = new SimulationCameraController(
274
"SimulationCameraController", camPos, freeCamState);
275
app.GetGameManager()->AddComponent(*cameraContr, dtGame::GameManager::ComponentPriority::LOWER);
277
cameraContr->init(guiComp->GetMouse(), guiComp->GetKeyboard());
279
cameraContr->init(app.GetMouse(), app.GetKeyboard());
282
#ifdef USE_SEPARATE_GUI
283
cameraContr->setViewport(false);
285
cameraContr->setViewport(true);
288
//Add Input Component
289
dtCore::RefPtr<SimulationInputComponent> inputComp = new SimulationInputComponent("SimulationInputComponent", withRobotControlEnabled);
290
app.GetGameManager()->AddComponent(*inputComp, dtGame::GameManager::ComponentPriority::LOWER);
292
//-----------------------------------------------------------------------------------------------------------------------
295
if (SimulationUtils::getWithShadows()) {
296
app.GetView(0)->GetOsgViewerView()->setSceneData(srCore::SimulationUtils::getShadowScene().get());
297
app.GetView(0)->GetOsgViewerView()->init();
300
std::vector<dtGame::GMComponent*> allComp;
301
app.GetGameManager()->GetAllComponents(allComp);
303
for (unsigned int i=0; i< allComp.size(); i++) {
304
if (dynamic_cast<dtCore::Light*>(allComp[i])) {
305
LOG_INFO("Found light (as component)");
309
LOG_INFO("Load all plugins from ComponentPath");
310
loadComp = new LoadComponents();
311
std::string comp_path = app.GetConfigPropertyValue("ComponentPath", DEFAULT_COMPONENT_PATH);
312
loadComp->SetLibPath(comp_path);
313
loadComp->GetLibraries();
314
loadComp->LoadLibs(app);
316
LOG_INFO("End of Robot3D simulator startup");
321
* Sets a bunch of parameters for the physics engine.
323
* @todo Move Physics related stuff to a separate component. The component may be build on
324
* dtPhysics, so it enables us to switch physics engines.
326
void SimulationEntryPoint::SetPhysicalParameters(dtGame::GameApplication &app) {
328
//Setting Collision NearCallback function
329
// void* data = NULL;
330
// app.GetScene()->SetUserCollisionCallback (SimulationUtils::NearCallback, data);
332
//Setting PhysicsStepSize and TargetFrameRate for stabilizing the simulation environment:
333
app.GetGameManager()->GetScene().SetPhysicsStepSize(0.05); //0.05
335
// The Constraint Force Mixing is invented by the ODE guys. It allows for relaxing of the
336
// constraint equation for joints. The original hard constraint is 0.
337
// @see <a>http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Constraint_Force_Mixing_.28CFM.29</a>
338
dWorldSetCFM(app.GetGameManager()->GetScene().GetWorldID(), 0.005); //0.05
340
// The Error Reduction Parameter is also invented by the ODE guys. It is for example
341
// possible to set the position of the multiple bodies that make up a robot in slightly
342
// wrong relative locations. Each joint can apply a force to bring its bodies back into
343
// the right positions.
344
// <a>@see http://opende.sourceforge.net/wiki/index.php/Manual_%28Concepts%29#Joint_error_and_the_error_reduction_parameter_.28ERP.29</a>
345
dWorldSetERP(app.GetGameManager()->GetScene().GetWorldID(), 0.9); //0.9
348
app.GetGameManager()->GetScene().SetGravity(0.f, 0.f, -GRAVITY);
350
// If you see a robot slowly sinking into the surface and then suddenly popping out in an
351
// incredibly high speed, that speed is set by this parameter. Of course, we normally don't
352
// want that to happen at all...
353
dWorldSetContactMaxCorrectingVel(app.GetGameManager()->GetScene().GetWorldID(), 250); //200
355
// This parameter tells how much the robots can sink into the ground, as described
356
// by the ODE lib, increasing this to some small value (e.g. 0.001) can help prevent
357
// jittering problems due to contacts being repeatedly made and broken.
358
dWorldSetContactSurfaceLayer (app.GetGameManager()->GetScene().GetWorldID(), 0.005); //0.001 (0.003)
360
app.GetGameManager()->ChangeTimeSettings(app.GetGameManager()->GetSimulationTime(), TIME_SCALE,
361
app.GetGameManager()->GetSimulationClockTime());
366
* Loads GUI components in the same dtCore::DeltaWin window, but using a different camera, a
367
* different view and a different scene.
368
* The GUI adds the information panel at the right. Where you will see information about the
369
* selected robot, the output of its sensors and the input to its actuators. It also adds a
370
* menu at the top to select certain components in the scene.
371
* @todo Freecam state or not is not the business of the simulationguicomponent
374
void SimulationEntryPoint::LoadGUIComponents(dtGame::GameApplication &app) {
375
// The default application view contains everything
376
dtCore::RefPtr<dtCore::View> appView = app.GetView();
377
dtCore::RefPtr<dtCore::Camera> appCamera = app.GetCamera();
378
dtCore::RefPtr<dtCore::Scene> appScene = app.GetScene();
379
dtCore::RefPtr<dtCore::DeltaWin> appWindow = app.GetWindow();
380
#ifdef USE_SEPARATE_GUI
381
// In this case not everything is added to the default application view
382
// The gui components are maintained in a separate guiView and guiScene and can be seen through a guiCamera
383
// This makes it possible for robots NOT to notice them
384
dtCore::RefPtr<dtCore::View> guiView;
385
dtCore::RefPtr<dtCore::Camera> guiCamera;
386
dtCore::RefPtr<dtCore::Scene> guiScene;
388
//Create also a new view and a new scene
389
LOG_INFO("Create a new view and a new scene for the GUI");
390
guiView = new dtCore::View("GUI View");
391
guiCamera = new dtCore::Camera("GUI Camera");
392
guiScene = new dtCore::Scene("GUI Scene");
394
// Set some default parameters
395
dtCore::Transform transform(20.0f, -30.0f, 15.0f);
396
guiCamera->SetTransform(transform);
397
guiCamera->SetClearColor(0.f, 0.f, 0.f, 0.f);
398
guiView->SetCamera(guiCamera);
400
// Add the view to the application
401
app.AddView(*guiView);
402
guiCamera->SetWindow(appWindow);
403
guiView->SetScene(guiScene);
404
guiCamera->GetOSGCamera()->setClearMask( GL_DEPTH_BUFFER_BIT );
406
//Load the CEGUI window layout
407
LOG_INFO("Load CEGUI window layout");
408
guiComp = new SimulationGUIComponent("SimulationGUIComponent", *appWindow,
409
*guiView->GetKeyboard(), *guiView->GetMouse());
411
// Create a new camera that sees the GUI
412
LOG_INFO("Create a new camera that sees the GUI (the default one will only see the scene with robots)");
413
guiScene->AddDrawable(guiComp->GetGUIDrawable());
415
// Adjust render order, so the GUI is rendered after the overall scene
416
app.GetView()->SetRenderOrder(1);
417
guiView->SetRenderOrder(app.GetView()->GetRenderOrder() + 1);
418
CEGUI::System::getSingleton().getGUISheet()->setAlpha(0.0f);
420
// Add component to game manager for later retrieval
421
app.GetGameManager()->AddComponent(*guiComp, dtGame::GameManager::ComponentPriority::LOWER);
423
//Load the CEGUI window layout
424
LOG_INFO("Load CEGUI window layout");
425
guiComp = new SimulationGUIComponent("SimulationGUIComponent", *appWindow,
426
*appView->GetKeyboard(), *appView->GetMouse());
429
app.GetScene()->AddDrawable(guiComp->GetGUIDrawable());
430
guiComp->GetGUIDrawable()->GetOSGNode()->setNodeMask(0x1);
432
app.GetGameManager()->AddComponent(*guiComp, dtGame::GameManager::ComponentPriority::LOWER);