void hormodular::SimulationOpenRAVE::init ( )
Loads the environment and sets everything needed.
{
    //-- Load the scene and wait for it to be ready:
    if ( !penv->Load( environment_file) )
    {
        std::cerr<< "[SimulationOpenRAVE] Error: could not load environment file!" << std::endl;
        std::cerr<< "Aborting program..." << std::endl;
        penv->Destroy();
        exit(-1);
    }
    usleep(100000);

    //-- Get the robot from the environment
    penv->GetRobots( robots );
    std::cout << "[Debug] Loaded " << robots.size() << " robots." << std::endl;

    //-- Lock the environment mutex:
    OpenRAVE::EnvironmentMutex::scoped_lock lock(penv->GetMutex());

    //-- Create a servocontroller and attach it to the robots:
    for( int i = 0; i < (int) robots.size(); i++)
    {
        //-- Create a new servocontroller
        OpenRAVE::ControllerBasePtr pcontroller = OpenRAVE::RaveCreateController(penv, "servocontroller");

        std::vector<int> dofindices( robots[i]->GetDOF());
        for (int j = 0; j < (int) dofindices.size(); j++)
            dofindices[j] = j;

        //-- Attach it
        robots[i]->SetController(pcontroller, dofindices, 1);

        std::cout << "[Debug] Robot " << i << ", number of joints: " << dofindices.size() << std::endl;
    }

    //-- Unlock the environment mutex:
    lock.unlock();

    //-- Reset controller and wait:
    for( int i = 0; i < (int) robots.size(); i++)
        robots[i]->GetController()->Reset();
}