YouBotManipulator.cpp

Go to the documentation of this file.

/****************************************************************
 *
 * Copyright (c) 2011
 * All rights reserved.
 *
 * Hochschule Bonn-Rhein-Sieg
 * University of Applied Sciences
 * Computer Science Department
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Author:
 * Jan Paulus, Nico Hochgeschwender, Michael Reckhaus, Azamat Shakhimardanov
 * Supervised by:
 * Gerhard K. Kraetzschmar
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * This sofware is published under a dual-license: GNU Lesser General Public 
 * License LGPL 2.1 and BSD license. The dual-license implies that users of this
 * code may choose which terms they prefer.
 *
 * +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Hochschule Bonn-Rhein-Sieg nor the names of its
 *       contributors may be used to endorse or promote products derived from
 *       this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 2.1 of the
 * License, or (at your option) any later version or the BSD license.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License LGPL and the BSD license for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL and BSD license along with this program.
 *
 ****************************************************************/
#include <youbot_driver/youbot/YouBotManipulator.hpp>
namespace youbot
{

YouBotManipulator::YouBotManipulator(const std::string name, const std::string configFilePath) :
    ethercatMaster(EthercatMaster::getInstance("youbot-ethercat.cfg", configFilePath))
{
  // Bouml preserved body begin 00067F71

  this->controllerType = 841;
  this->alternativeControllerType = 1610;
  this->supportedFirmwareVersions.push_back("148");
  this->supportedFirmwareVersions.push_back("200");
  this->actualFirmwareVersionAllJoints = "";

  string filename;
  filename = name;
  filename.append(".cfg");
  useGripper = true;
  configfile.reset(new ConfigFile(filename, configFilePath));

  if (ethercatMaster.isThreadActive())
  {
    ethercatMasterWithThread = static_cast<EthercatMasterWithThread*>(&(EthercatMaster::getInstance(filename,
                                                                                                    configFilePath,
                                                                                                    true)));
  }
  else
  {
    ethercatMasterWithThread = NULL;
  }

  this->initializeJoints();

  // Bouml preserved body end 00067F71
}

YouBotManipulator::~YouBotManipulator()
{
  // Bouml preserved body begin 00067FF1
  if (ethercatMaster.isThreadActive())
  {
    for (unsigned int i = 0; i < ARMJOINTS; i++)
    {
      ethercatMasterWithThread->deleteJointTrajectoryControllerRegistration(this->joints[i].getJointNumber());
    }
  }
  // Bouml preserved body end 00067FF1
}

void YouBotManipulator::doJointCommutation()
{
  // Bouml preserved body begin 000A3371

  if (this->actualFirmwareVersionAllJoints == "148")
  {
    this->commutationFirmware148();
  }
  else if (this->actualFirmwareVersionAllJoints == "200")
  {
    this->commutationFirmware200();
  }
  else
  {
    throw std::runtime_error("Unable to commutate joints - Unsupported firmware version!");
  }
  // Bouml preserved body end 000A3371
}

void YouBotManipulator::calibrateManipulator(const bool forceCalibration)
{
  // Bouml preserved body begin 000A9C71

  //Calibrate all manipulator joints
  std::vector<JointRoundsPerMinuteSetpoint> calibrationVel;
  JointRoundsPerMinuteSetpoint tempdummy;
  tempdummy.rpm = 0;
  calibrationVel.assign(ARMJOINTS, tempdummy);
  std::vector<quantity<si::current> > maxCurrent;
  quantity<si::current> tempdummy2;
  maxCurrent.assign(ARMJOINTS, tempdummy2);
  std::vector<bool> doCalibration;
  doCalibration.assign(ARMJOINTS, true);
  std::string jointName;

  double dummy = 0;
  char index = 16; // Parameter 0 to 15 of bank 2 are password protected

  YouBotSlaveMailboxMsg IsCalibratedReadMessage;
  IsCalibratedReadMessage.stctOutput.moduleAddress = DRIVE;
  IsCalibratedReadMessage.stctOutput.commandNumber = GGP;
  IsCalibratedReadMessage.stctOutput.typeNumber = index;
  IsCalibratedReadMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
  IsCalibratedReadMessage.stctOutput.value = 0;
  IsCalibratedReadMessage.stctInput.value = 0;

  YouBotSlaveMailboxMsg IsCalibratedSetMessage;
  IsCalibratedSetMessage.stctOutput.moduleAddress = DRIVE;
  IsCalibratedSetMessage.stctOutput.commandNumber = SGP;
  IsCalibratedSetMessage.stctOutput.typeNumber = index;
  IsCalibratedSetMessage.stctOutput.motorNumber = USER_VARIABLE_BANK;
  IsCalibratedSetMessage.stctOutput.value = 1;

  //get parameters for calibration
  for (unsigned int i = 0; i < ARMJOINTS; i++)
  {

    std::stringstream jointNameStream;
    jointNameStream << "Joint_" << i + 1;
    jointName = jointNameStream.str();
    bool calib = true;
    configfile->readInto(calib, jointName, "DoCalibration");
    doCalibration[i] = calib;

    joints[i].getConfigurationParameter(IsCalibratedReadMessage);
    if (IsCalibratedReadMessage.stctInput.value == 1)
    {
      doCalibration[i] = false;
    }

    if (forceCalibration)
    {
      doCalibration[i] = true;
    }

    configfile->readInto(dummy, jointName, "CalibrationMaxCurrent_[ampere]");
    maxCurrent[i] = dummy * ampere;
    std::string direction;
    configfile->readInto(direction, jointName, "CalibrationDirection");
    GearRatio gearRatio;
    joints[i].getConfigurationParameter(gearRatio);
    double gearratio = 1;
    gearRatio.getParameter(gearratio);

    if (direction == "POSITIV")
    {
      calibrationVel[i].rpm = 1 / gearratio;
    }
    else if (direction == "NEGATIV")
    {
      calibrationVel[i].rpm = -1 / gearratio;
    }
    else
    {
      throw std::runtime_error("Wrong calibration direction for " + jointName);
    }
  }

  LOG(info) << "Calibrate Manipulator Joints ";

  std::vector<bool> finished;
  finished.assign(ARMJOINTS, false);
  JointSensedCurrent sensedCurrent;

  //move the joints slowly in calibration direction
  for (unsigned int i = 0; i < ARMJOINTS; i++)
  {
    if (doCalibration[i] == true)
    {
      joints[i].setData(calibrationVel[i]);
      if (!ethercatMaster.isThreadActive())
      {
        ethercatMaster.sendProcessData();
        ethercatMaster.receiveProcessData();
      }
    }
    else
    {
      finished[i] = true;
    }
  }

  //monitor the current to find end stop
  while (!(finished[0] && finished[1] && finished[2] && finished[3] && finished[4]))
  {
    for (unsigned int i = 0; i < ARMJOINTS; i++)
    {
      if (!ethercatMaster.isThreadActive())
      {
        ethercatMaster.sendProcessData();
        ethercatMaster.receiveProcessData();
      }
      joints[i].getData(sensedCurrent);
      //turn till a max current is reached
      if (abs(sensedCurrent.current) > abs(maxCurrent[i]))
      {
        //stop movement
        youbot::JointCurrentSetpoint currentStopMovement;
        currentStopMovement.current = 0 * ampere;
        joints[i].setData(currentStopMovement);
        if (!ethercatMaster.isThreadActive())
        {
          ethercatMaster.sendProcessData();
          ethercatMaster.receiveProcessData();
        }
        finished[i] = true;
      }
    }
    SLEEP_MILLISEC(1);
  }

  // wait to let the joint stop the motion
  SLEEP_MILLISEC(100);

  for (unsigned int i = 0; i < ARMJOINTS; i++)
  {
    if (doCalibration[i] == true)
    {
      //set encoder reference position
      joints[i].setEncoderToZero();
      if (!ethercatMaster.isThreadActive())
      {
        ethercatMaster.sendProcessData();
        ethercatMaster.receiveProcessData();
      }
      // set a flag in the user variable to remember that it is calibrated
      joints[i].setConfigurationParameter(IsCalibratedSetMessage);
      //     LOG(info) << "Calibration finished for joint: " << this->jointName;
    }
  }

  //setting joint Limits
  JointLimits jLimits;
  for (unsigned int i = 0; i < ARMJOINTS; i++)
  {
    long upperlimit = 0, lowerlimit = 0;
    std::stringstream jointNameStream;
    bool inverted = false;
    jointNameStream << "Joint_" << i + 1;
    jointName = jointNameStream.str();
    JointEncoderSetpoint minEncoderValue;
    configfile->readInto(lowerlimit, jointName, "LowerLimit_[encoderTicks]");
    configfile->readInto(upperlimit, jointName, "UpperLimit_[encoderTicks]");
    configfile->readInto(inverted, jointName, "InverseMovementDirection");

    if (inverted)
    {
      minEncoderValue.encoderTicks = lowerlimit + 1000;
    }
    else
    {
      minEncoderValue.encoderTicks = upperlimit - 1000;
    }

    jLimits.setParameter(lowerlimit, upperlimit, true);
    joints[i].setConfigurationParameter(jLimits);
    // joints[i].setData(minEncoderValue);
  }

  // Bouml preserved body end 000A9C71
}

void YouBotManipulator::calibrateGripper(const bool forceCalibration)
{
  // Bouml preserved body begin 000A9CF1
  // Calibrating Gripper
  bool doCalibration = true;
  configfile->readInto(doCalibration, "Gripper", "DoCalibration");
  if (useGripper && doCalibration)
  {
    CalibrateGripper calibrate;
    calibrate.setParameter(forceCalibration);
    gripper->setConfigurationParameter(calibrate);
  }
  // Bouml preserved body end 000A9CF1
}

YouBotJoint& YouBotManipulator::getArmJoint(const unsigned int armJointNumber)
{
  // Bouml preserved body begin 0004F7F1

  if (armJointNumber <= 0 || armJointNumber > ARMJOINTS)
  {
    throw std::out_of_range("Invalid Joint Number");
  }
  return joints[armJointNumber - 1];
  // Bouml preserved body end 0004F7F1
}

YouBotGripper& YouBotManipulator::getArmGripper()
{
  // Bouml preserved body begin 0005F9F1
  if (!useGripper)
  {
    throw std::runtime_error("The gripper is disabled!");
  }
  return *gripper;
  // Bouml preserved body end 0005F9F1
}

void YouBotManipulator::setJointData(const std::vector<JointAngleSetpoint>& JointData)
{
  // Bouml preserved body begin 0008FDF1
  if (JointData.size() != ARMJOINTS)
    throw std::out_of_range("Wrong number of JointAngleSetpoints");

  ethercatMaster.AutomaticSendOn(false);
  joints[0].setData(JointData[0]);
  joints[1].setData(JointData[1]);
  joints[2].setData(JointData[2]);
  joints[3].setData(JointData[3]);
  joints[4].setData(JointData[4]);
  ethercatMaster.AutomaticSendOn(true);

  // Bouml preserved body end 0008FDF1
}

void YouBotManipulator::getJointData(std::vector<JointSensedAngle>& data)
{
  // Bouml preserved body begin 0008FE71
  data.resize(ARMJOINTS);
  ethercatMaster.AutomaticReceiveOn(false);
  joints[0].getData(data[0]);
  joints[1].getData(data[1]);
  joints[2].getData(data[2]);
  joints[3].getData(data[3]);
  joints[4].getData(data[4]);
  ethercatMaster.AutomaticReceiveOn(true);
  // Bouml preserved body end 0008FE71
}

void YouBotManipulator::setJointData(const std::vector<JointVelocitySetpoint>& JointData)
{
  // Bouml preserved body begin 0008FEF1
  if (JointData.size() != ARMJOINTS)
    throw std::out_of_range("Wrong number of JointVelocitySetpoints");

  ethercatMaster.AutomaticSendOn(false);
  joints[0].setData(JointData[0]);
  joints[1].setData(JointData[1]);
  joints[2].setData(JointData[2]);
  joints[3].setData(JointData[3]);
  joints[4].setData(JointData[4]);
  ethercatMaster.AutomaticSendOn(true);
  // Bouml preserved body end 0008FEF1
}

void YouBotManipulator::getJointData(std::vector<JointSensedVelocity>& data)
{
  // Bouml preserved body begin 0008FF71
  data.resize(ARMJOINTS);
  ethercatMaster.AutomaticReceiveOn(false);
  joints[0].getData(data[0]);
  joints[1].getData(data[1]);
  joints[2].getData(data[2]);
  joints[3].getData(data[3]);
  joints[4].getData(data[4]);
  ethercatMaster.AutomaticReceiveOn(true);
  // Bouml preserved body end 0008FF71
}

void YouBotManipulator::setJointData(const std::vector<JointCurrentSetpoint>& JointData)
{
  // Bouml preserved body begin 000CDE71
  if (JointData.size() != ARMJOINTS)
    throw std::out_of_range("Wrong number of JointCurrentSetpoint");

  ethercatMaster.AutomaticSendOn(false);
  joints[0].setData(JointData[0]);
  joints[1].setData(JointData[1]);
  joints[2].setData(JointData[2]);
  joints[3].setData(JointData[3]);
  joints[4].setData(JointData[4]);
  ethercatMaster.AutomaticSendOn(true);
  // Bouml preserved body end 000CDE71
}

void YouBotManipulator::getJointData(std::vector<JointSensedCurrent>& data)
{
  // Bouml preserved body begin 00090071
  data.resize(ARMJOINTS);
  ethercatMaster.AutomaticReceiveOn(false);
  joints[0].getData(data[0]);
  joints[1].getData(data[1]);
  joints[2].getData(data[2]);
  joints[3].getData(data[3]);
  joints[4].getData(data[4]);
  ethercatMaster.AutomaticReceiveOn(true);
  // Bouml preserved body end 00090071
}

void YouBotManipulator::setJointData(const std::vector<JointTorqueSetpoint>& JointData)
{
  // Bouml preserved body begin 000CDEF1
  if (JointData.size() != ARMJOINTS)
    throw std::out_of_range("Wrong number of JointTorqueSetpoint");

  ethercatMaster.AutomaticSendOn(false);
  joints[0].setData(JointData[0]);
  joints[1].setData(JointData[1]);
  joints[2].setData(JointData[2]);
  joints[3].setData(JointData[3]);
  joints[4].setData(JointData[4]);
  ethercatMaster.AutomaticSendOn(true);
  // Bouml preserved body end 000CDEF1
}

void YouBotManipulator::getJointData(std::vector<JointSensedTorque>& data)
{
  // Bouml preserved body begin 000CDF71
  data.resize(ARMJOINTS);
  ethercatMaster.AutomaticReceiveOn(false);
  joints[0].getData(data[0]);
  joints[1].getData(data[1]);
  joints[2].getData(data[2]);
  joints[3].getData(data[3]);
  joints[4].getData(data[4]);
  ethercatMaster.AutomaticReceiveOn(true);
  // Bouml preserved body end 000CDF71
}

void YouBotManipulator::commutationFirmware200()
{
  // Bouml preserved body begin 0010D8F1

  InitializeJoint doInitialization;
  bool isInitialized = false;
  int noInitialization = 0;
  std::string jointName;
  unsigned int statusFlags;
  std::vector<bool> isCommutated;
  isCommutated.assign(ARMJOINTS, false);
  unsigned int u = 0;
  JointCurrentSetpoint zerocurrent;
  zerocurrent.current = 0.0 * ampere;

  ClearMotorControllerTimeoutFlag clearTimeoutFlag;
  for (unsigned int i = 1; i <= ARMJOINTS; i++)
  {
    this->getArmJoint(i).setConfigurationParameter(clearTimeoutFlag);
  }

  for (unsigned int i = 1; i <= ARMJOINTS; i++)
  {
    doInitialization.setParameter(false);
    this->getArmJoint(i).getConfigurationParameter(doInitialization);
    doInitialization.getParameter(isInitialized);
    if (!isInitialized)
    {
      noInitialization++;
    }
  }

  if (noInitialization != 0)
  {
    LOG(info) << "Manipulator Joint Commutation";
    doInitialization.setParameter(true);

    JointRoundsPerMinuteSetpoint rpmSetpoint(100);

    ethercatMaster.AutomaticReceiveOn(false);
    this->getArmJoint(1).setData(rpmSetpoint);
    this->getArmJoint(2).setData(rpmSetpoint);
    this->getArmJoint(3).setData(rpmSetpoint);
    this->getArmJoint(4).setData(rpmSetpoint);
    this->getArmJoint(5).setData(rpmSetpoint);
    ethercatMaster.AutomaticReceiveOn(true);

    // check for the next 5 sec if the joints are commutated
    for (u = 1; u <= 5000; u++)
    {
      for (unsigned int i = 1; i <= ARMJOINTS; i++)
      {
        this->getArmJoint(i).getStatus(statusFlags);
        if (statusFlags & INITIALIZED)
        {
          isCommutated[i - 1] = true;
          this->getArmJoint(i).setData(zerocurrent);
        }
      }
      if (!ethercatMaster.isThreadActive())
      {
        ethercatMaster.sendProcessData();
        ethercatMaster.receiveProcessData();
      }
      if (isCommutated[0] && isCommutated[1] && isCommutated[2] && isCommutated[3] && isCommutated[4])
      {
        break;
      }
      SLEEP_MILLISEC(1);
    }

    for (unsigned int i = 1; i <= ARMJOINTS; i++)
    {
      this->getArmJoint(i).setData(zerocurrent);
      if (!ethercatMaster.isThreadActive())
      {
        ethercatMaster.sendProcessData();
        ethercatMaster.receiveProcessData();
      }
      doInitialization.setParameter(false);
      this->getArmJoint(i).getConfigurationParameter(doInitialization);
      doInitialization.getParameter(isInitialized);
      if (!isInitialized)
      {
        std::stringstream jointNameStream;
        jointNameStream << "manipulator joint " << i;
        jointName = jointNameStream.str();
        throw std::runtime_error("Could not commutate " + jointName);
      }
    }
  }
  // Bouml preserved body end 0010D8F1
}

void YouBotManipulator::commutationFirmware148()
{
  // Bouml preserved body begin 0010D971

  InitializeJoint doInitialization;
  bool isInitialized = false;
  int noInitialization = 0;
  std::string jointName;

  ClearMotorControllerTimeoutFlag clearTimeoutFlag;
  for (unsigned int i = 1; i <= ARMJOINTS; i++)
  {
    this->getArmJoint(i).setConfigurationParameter(clearTimeoutFlag);
  }

  for (unsigned int i = 1; i <= ARMJOINTS; i++)
  {
    doInitialization.setParameter(false);
    this->getArmJoint(i).getConfigurationParameter(doInitialization);
    doInitialization.getParameter(isInitialized);
    if (!isInitialized)
    {
      noInitialization++;
    }
  }

  if (noInitialization != 0)
  {
    LOG(info) << "Manipulator Joint Commutation";
    doInitialization.setParameter(true);

    ethercatMaster.AutomaticReceiveOn(false);
    this->getArmJoint(1).setConfigurationParameter(doInitialization);
    this->getArmJoint(2).setConfigurationParameter(doInitialization);
    this->getArmJoint(3).setConfigurationParameter(doInitialization);
    this->getArmJoint(4).setConfigurationParameter(doInitialization);
    this->getArmJoint(5).setConfigurationParameter(doInitialization);
    ethercatMaster.AutomaticReceiveOn(true);

    unsigned int statusFlags;
    std::vector<bool> isCommutated;
    isCommutated.assign(ARMJOINTS, false);
    unsigned int u = 0;

    // check for the next 5 sec if the joints are commutated
    for (u = 1; u <= 5000; u++)
    {
      for (unsigned int i = 1; i <= ARMJOINTS; i++)
      {
        if (!ethercatMaster.isThreadActive())
        {
          ethercatMaster.sendProcessData();
          ethercatMaster.receiveProcessData();
        }
        this->getArmJoint(i).getStatus(statusFlags);
        if (statusFlags & INITIALIZED)
        {
          isCommutated[i - 1] = true;
        }
      }
      if (isCommutated[0] && isCommutated[1] && isCommutated[2] && isCommutated[3] && isCommutated[4])
      {
        break;
      }
      SLEEP_MILLISEC(1);
    }

    SLEEP_MILLISEC(10); // the controller likes it

    for (unsigned int i = 1; i <= ARMJOINTS; i++)
    {
      doInitialization.setParameter(false);
      this->getArmJoint(i).getConfigurationParameter(doInitialization);
      doInitialization.getParameter(isInitialized);
      if (!isInitialized)
      {
        std::stringstream jointNameStream;
        jointNameStream << "manipulator joint " << i;
        jointName = jointNameStream.str();
        throw std::runtime_error("Could not commutate " + jointName);
      }
    }
  }

  // Bouml preserved body end 0010D971
}

void YouBotManipulator::initializeJoints()
{
  // Bouml preserved body begin 00068071

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

  char* configLocation = getenv("YOUBOT_CONFIG_FOLDER_LOCATION");
  if (configLocation == NULL)
    throw std::runtime_error("YouBotArmTest.cpp: Could not find environment variable YOUBOT_CONFIG_FOLDER_LOCATION");

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

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

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

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

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

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

  configfile->readInto(slaveNumber, "JointTopology", "ManipulatorJoint5");
  if (slaveNumber <= noSlaves)
  {
    joints.push_back(new YouBotJoint(slaveNumber, configLocation));
  }
  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 < ARMJOINTS; 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, configLocation));
    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
}


bool YouBotManipulator::isEtherCATConnectionEstablished() {
  return (ethercatMasterWithThread!=NULL && ethercatMasterWithThread->isEtherCATConnectionEstablished());
}

} // namespace youbot