void YouBotDiagnostics::getAllTopologicalInformation()
{
    bool reset = true;

    unsigned jointNo  = 0;
    unsigned jointCfg = 0;

    //joint konfiguration variables
    JointName jName;
    GearRatio gearRatio;
    EncoderTicksPerRound ticksPerRound;
    InverseMovementDirection inverseDir;
    JointLimits jLimits;
    FirmwareVersion firmwareTypeVersion;

    ticksPerRound.setParameter(4000);
    int controllerType;
    string firmwareVersion;

    *outputFile << "----- Topological Information of all slaves: -----" << endl;

    for (int i = 0, slaveCtr = 1; i < etherCATSlaves.size(); i++, slaveCtr++)
    {
        //formatted output:
        *outputFile << "slave no "	<< setw(2) << slaveCtr << ":\t"	<< slaveTypMap[etherCATSlaves[i].name];

//		if( slaveTypMap[etherCATSlaves[i].name] == "BASE-Master")
//			*outputFile << ": " << etherCATSlaves[i].name;
//		else
        *outputFile << ":\t" << etherCATSlaves[i].name;

        if ( slaveTypMap[etherCATSlaves[i].name] == "BASE-Master" || slaveTypMap[etherCATSlaves[i].name] == "ARM-Master")
            *outputFile << "\t\t--> parent: ";
        else
            *outputFile << "\t--> parent: ";

        *outputFile << setw(2) << etherCATSlaves[i].parent << "\t--> connections: " << setw(2) << (int) etherCATSlaves[i].topology << endl;


        //create the joint map
        if (reset)
            jointCfg = 1;
        else
            jointCfg++;

        if ( !strcmp( etherCATSlaves[i].name, BASE_MASTER_BOARD) )
        {
            reset = true;
            noOfBaseMasterBoards++;
            motorActivity[slaveCtr] = true;
        }
        if ( !strcmp( etherCATSlaves[i].name, ARM_MASTER_BOARD ) )
        {
            reset = true;
            noOfArmMasterBoards++;
            motorActivity[slaveCtr] = true;
        }

        if ( !strcmp( etherCATSlaves[i].name, BASE_CONTROLLER  ) )
        {
            noOfBaseSlaveControllers++;
            jointNo++;
            reset = false;

            TopologyMap::iterator it = youBotJointMap.insert( pair<int, JointTopology>( slaveCtr, JointTopology() ) ).first;
            it->second.relatedUnit          = string("BASE");
            it->second.relatedMasterBoard   = string(BASE_MASTER_BOARD);
            it->second.controllerTyp        = string(BASE_CONTROLLER);
            it->second.masterBoardNo        = noOfBaseMasterBoards;
            it->second.slaveNo              = slaveCtr;
            it->second.jointTopologyNo      = jointNo;
            it->second.jointConfigNo        = jointCfg;
            it->second.joint                = new YouBotJoint(jointNo);

            it->second.joint->getConfigurationParameter(firmwareTypeVersion);
            firmwareTypeVersion.getParameter(controllerType, firmwareVersion);
            it->second.firmwareVersion = atof(firmwareVersion.c_str());

            stringstream name;
            name << "joint_" << jointNo;
            jName.setParameter( name.str() );

            gearRatio.setParameter(364.0/9405.0);
            inverseDir.setParameter(false);

            it->second.joint->setConfigurationParameter(jName);
            it->second.joint->setConfigurationParameter(gearRatio);
            it->second.joint->setConfigurationParameter(ticksPerRound);
            it->second.joint->setConfigurationParameter(inverseDir);
        }

        if( !strcmp( etherCATSlaves[i].name, ARM_CONTROLLER   ) )
        {
            noOfArmSlaveControllers++;
            jointNo++;
            reset = false;

            TopologyMap::iterator it = youBotJointMap.insert( pair<int, JointTopology>( slaveCtr, JointTopology() ) ).first;
            it->second.relatedUnit		  = string("MANIPULATOR");
            it->second.relatedMasterBoard = string(ARM_MASTER_BOARD);
            it->second.controllerTyp	  = string(ARM_CONTROLLER);
            it->second.masterBoardNo	  = noOfArmMasterBoards;
            it->second.slaveNo			  = slaveCtr;
            it->second.jointTopologyNo	  = jointNo;
            it->second.jointConfigNo      = jointCfg;
            it->second.joint		      = new YouBotJoint(jointNo);

            it->second.joint->getConfigurationParameter(firmwareTypeVersion);
            firmwareTypeVersion.getParameter(controllerType, firmwareVersion);
            it->second.firmwareVersion = atof(firmwareVersion.c_str());

            stringstream name;
            name << "joint_" << jointNo;
            jName.setParameter( name.str() );


            switch (jointCfg)
            {
            case 1:
                gearRatio.setParameter(1.0/156.0);
                inverseDir.setParameter(true);
                jLimits.setParameter(-580000, -1000, true);
                break;

            case 2:
                gearRatio.setParameter(1.0/156.0);
                inverseDir.setParameter(true);
                jLimits.setParameter(-260000, -1000, true);
                break;

            case 3:
                gearRatio.setParameter(1.0/100.0);
                inverseDir.setParameter(true);
                jLimits.setParameter(-320000, -1000, true);
                break;

            case 4:
                gearRatio.setParameter(1.0/ 71.0);
                inverseDir.setParameter(true);
                jLimits.setParameter(-155000, -1000, true);
                break;

            case 5:
                gearRatio.setParameter(1.0/ 71.0);
                inverseDir.setParameter(true);
                jLimits.setParameter(-255000, -1000, true);
                break;
            }


            it->second.joint->setConfigurationParameter(jName);
            it->second.joint->setConfigurationParameter(ticksPerRound);
            it->second.joint->setConfigurationParameter(inverseDir);
            it->second.joint->setConfigurationParameter(jLimits);
            it->second.joint->setConfigurationParameter(gearRatio);
        }
    }

    *outputFile << separator;
    *outputFile << "Number of ALL slaves:           "  << etherCATSlaves.size() << endl;
    *outputFile << "Number of JOINT slaves:         "  << youBotJointMap.size()  << endl;
    *outputFile << "Number of BaseMasterBoards:     "  << noOfBaseMasterBoards << endl;
    *outputFile << "Number of BaseSlaveControllers: "  << noOfBaseSlaveControllers << endl;
    *outputFile << "Number of ArmMasterBoards:      "  << noOfArmMasterBoards << endl;
    *outputFile << "Number of ArmSlaveControllers:  "  << noOfArmSlaveControllers << endl;
    *outputFile << separator;
}
void YouBotManipulator::initializeJoints() {
  // Bouml preserved body begin 00068071

 //   LOG(info) << "Initializing Joints";

    //enable overriding the number of joints
    if (configfile->keyExists("JointTopology", "NumberJoints"))
        configfile->readInto(numberArmJoints, "JointTopology", "NumberJoints");

    //get number of slaves
    unsigned int noSlaves = ethercatMaster.getNumberOfSlaves();

    if (noSlaves < numberArmJoints) {
      throw std::runtime_error("Not enough ethercat slaves were found to create a YouBotManipulator!");
    }

    unsigned int slaveNumber = 0;
    for (unsigned int i = 1; i <= numberArmJoints; i++) {
		std::stringstream jointConfigNameStream;
		jointConfigNameStream << "ManipulatorJoint" << i;
		std::string jointConfigName = jointConfigNameStream.str();

		configfile->readInto(slaveNumber, "JointTopology", jointConfigName);
		if (slaveNumber <= noSlaves) {
			joints.push_back(new YouBotJoint(slaveNumber));
		} else {
			throw std::out_of_range("The ethercat slave number is not available!");
		}
	}

    //Configure Joint Parameters
    std::string jointName;
    JointName jName;
    GearRatio gearRatio;
    EncoderTicksPerRound ticksPerRound;
    InverseMovementDirection inverseDir;
    double gearRatio_numerator = 0;
    double gearRatio_denominator = 1;
    FirmwareVersion firmwareTypeVersion;
    TorqueConstant torqueConst;
    double trajectory_p=0, trajectory_i=0, trajectory_d=0, trajectory_imax=0, trajectory_imin=0;

    for (unsigned int i = 0; i < numberArmJoints; i++) {
      std::stringstream jointNameStream;
      jointNameStream << "Joint_" << i + 1;
      jointName = jointNameStream.str();


      joints[i].getConfigurationParameter(firmwareTypeVersion);
      std::string version;
      int controllerType;
      std::string firmwareVersion;
      firmwareTypeVersion.getParameter(controllerType, firmwareVersion);

      string name;
      configfile->readInto(name, jointName, "JointName");
      jName.setParameter(name);

      LOG(info) << name << "\t Controller Type: " << controllerType << "  Firmware version: " << firmwareVersion;

      if (this->controllerType != controllerType && alternativeControllerType != controllerType) {
        std::stringstream ss;
        ss << "The youBot manipulator motor controller have to be of type: " << this->controllerType << " or " << alternativeControllerType;
        throw std::runtime_error(ss.str().c_str());
      }

      //check if firmware is supported
      bool isfirmwareSupported = false;
      for(unsigned int d = 0; d < supportedFirmwareVersions.size(); d++){
        if(this->supportedFirmwareVersions[d] == firmwareVersion){
          isfirmwareSupported = true;
          break;
        }
      }
      
      if(!isfirmwareSupported){
        throw std::runtime_error("Unsupported firmware version: " + firmwareVersion);
      }
      
      if(this->actualFirmwareVersionAllJoints == ""){
        this->actualFirmwareVersionAllJoints = firmwareVersion;
      }
      
      if(!(firmwareVersion == this->actualFirmwareVersionAllJoints)){
         throw std::runtime_error("All joints must have the same firmware version!");
      }

      configfile->readInto(gearRatio_numerator, jointName, "GearRatio_numerator");
      configfile->readInto(gearRatio_denominator, jointName, "GearRatio_denominator");
      gearRatio.setParameter(gearRatio_numerator / gearRatio_denominator);
      int ticks;
      configfile->readInto(ticks, jointName, "EncoderTicksPerRound");
      ticksPerRound.setParameter(ticks);
      
      double torqueConstant;
      configfile->readInto(torqueConstant, jointName, "TorqueConstant_[newton_meter_divided_by_ampere]");
      torqueConst.setParameter(torqueConstant);
      
      bool invdir = false;
      configfile->readInto(invdir, jointName, "InverseMovementDirection");
      inverseDir.setParameter(invdir);

      joints[i].setConfigurationParameter(jName);
      joints[i].setConfigurationParameter(gearRatio);
      joints[i].setConfigurationParameter(ticksPerRound);
      joints[i].setConfigurationParameter(torqueConst);
      joints[i].setConfigurationParameter(inverseDir);
      
      //Joint Trajectory Controller
      if(ethercatMaster.isThreadActive()){
				configfile->readInto(trajectory_p, jointName, "trajectory_controller_P");
        configfile->readInto(trajectory_i, jointName, "trajectory_controller_I");
        configfile->readInto(trajectory_d, jointName, "trajectory_controller_D");
        configfile->readInto(trajectory_imax, jointName, "trajectory_controller_I_max");
        configfile->readInto(trajectory_imin, jointName, "trajectory_controller_I_min");
        joints[i].trajectoryController.setConfigurationParameter(trajectory_p, trajectory_i, trajectory_d, trajectory_imax, trajectory_imin);
				joints[i].trajectoryController.setEncoderTicksPerRound(ticks);
        joints[i].trajectoryController.setGearRatio(gearRatio_numerator / gearRatio_denominator);
        joints[i].trajectoryController.setInverseMovementDirection(invdir);
        ethercatMasterWithThread->registerJointTrajectoryController(&(joints[i].trajectoryController), joints[i].getJointNumber());
			}
    }


		configfile->readInto(useGripper, "Gripper", "EnableGripper");
		
    if(useGripper){
    //Initializing Gripper
    configfile->readInto(slaveNumber, "JointTopology", "ManipulatorJoint5");
    this->gripper.reset(new YouBotGripper(slaveNumber));
    BarSpacingOffset barOffest;
    MaxTravelDistance maxDistance;
    MaxEncoderValue maxEncoder;
    GripperBarName BarName;
    double dummy = 0;
    int controllerType;
    double firmwareVersion;
    string barname;
    
    GripperFirmwareVersion gripperVersion;
    this->gripper->getConfigurationParameter(gripperVersion);
    gripperVersion.getParameter(controllerType, firmwareVersion);
    
    LOG(info) << "Gripper" << "\t\t Controller Type: " << controllerType << "  Firmware version: " << firmwareVersion;

    // Gripper Bar 1
    configfile->readInto(barname, "GripperBar1", "BarName");
    BarName.setParameter(barname);
    this->gripper->getGripperBar1().setConfigurationParameter(BarName);
    
    configfile->readInto(dummy, "GripperBar1", "BarSpacingOffset_[meter]");
    barOffest.setParameter(dummy * meter);
    this->gripper->getGripperBar1().setConfigurationParameter(barOffest);
    
    configfile->readInto(dummy, "GripperBar1", "MaxTravelDistance_[meter]");
    maxDistance.setParameter(dummy * meter);
    this->gripper->getGripperBar1().setConfigurationParameter(maxDistance);
    
    int maxenc = 0;
    configfile->readInto(maxenc, "GripperBar1", "MaxEncoderValue");
    maxEncoder.setParameter(maxenc);
    this->gripper->getGripperBar1().setConfigurationParameter(maxEncoder);
    
    int stallThreshold = 0;
    configfile->readInto(stallThreshold, "GripperBar1", "StallGuard2Threshold");
    StallGuard2Threshold threshold;
    threshold.setParameter(stallThreshold);
    this->gripper->getGripperBar1().setConfigurationParameter(threshold);

    bool stallGuardFilter = false;
    configfile->readInto(stallGuardFilter, "GripperBar1", "StallGuard2FilterEnable");
    StallGuard2FilterEnable filter;
    filter.setParameter(stallGuardFilter);
    this->gripper->getGripperBar1().setConfigurationParameter(filter);
    
    // Gripper Bar 2
    configfile->readInto(barname, "GripperBar2", "BarName");
    BarName.setParameter(barname);
    this->gripper->getGripperBar2().setConfigurationParameter(BarName);
    
    configfile->readInto(dummy, "GripperBar2", "BarSpacingOffset_[meter]");
    barOffest.setParameter(dummy * meter);
    this->gripper->getGripperBar2().setConfigurationParameter(barOffest);
    
    configfile->readInto(dummy, "GripperBar2", "MaxTravelDistance_[meter]");
    maxDistance.setParameter(dummy * meter);
    this->gripper->getGripperBar2().setConfigurationParameter(maxDistance);

    configfile->readInto(maxenc, "GripperBar2", "MaxEncoderValue");
    maxEncoder.setParameter(maxenc);
    this->gripper->getGripperBar2().setConfigurationParameter(maxEncoder);
    
    configfile->readInto(stallThreshold, "GripperBar2", "StallGuard2Threshold");
    threshold.setParameter(stallThreshold);
    this->gripper->getGripperBar2().setConfigurationParameter(threshold);

    configfile->readInto(stallGuardFilter, "GripperBar2", "StallGuard2FilterEnable");
    filter.setParameter(stallGuardFilter);
    this->gripper->getGripperBar2().setConfigurationParameter(filter);
    }
    

    return;
  // Bouml preserved body end 00068071
}