PowerCubeCtrl.cpp

Go to the documentation of this file.

/*
 * Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *   http://www.apache.org/licenses/LICENSE-2.0

 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */


// ROS includes
#include <ros/ros.h>

// own includes
#include <schunk_powercube_chain/PowerCubeCtrl.h>

#define PCTRL_CHECK_INITIALIZED()                          \
  if (isInitialized() == false)                            \
  {                                                        \
    m_ErrorMessage.assign("Manipulator not initialized."); \
    return false;                                          \
  }

#define Ready4MoveStep 4638

/*
 * \brief Constructor
 */
PowerCubeCtrl::PowerCubeCtrl(PowerCubeCtrlParams* params)
{
  m_mutex = PTHREAD_MUTEX_INITIALIZER;

  m_CANDeviceOpened = false;
  m_Initialized = false;

  m_params = params;

  m_horizon = 0.01;  // sec

  m_last_time_pub = ros::Time::now();

  m_pc_status = PC_CTRL_OK;
}

/*
 * \brief Destructor
 */
PowerCubeCtrl::~PowerCubeCtrl()
{
  // stop all components
  Stop();

  // close CAN device
  if (m_CANDeviceOpened)
  {
    pthread_mutex_lock(&m_mutex);
    PCube_closeDevice(m_DeviceHandle);
    pthread_mutex_unlock(&m_mutex);
  }
}


bool PowerCubeCtrl::Init(PowerCubeCtrlParams* params)
{
  int ret = 0;
  int DOF = m_params->GetDOF();
  std::string CanModule = m_params->GetCanModule();
  std::string CanDevice = m_params->GetCanDevice();
  std::vector<int> ModulIDs = m_params->GetModuleIDs();
  int CanBaudrate = m_params->GetBaudrate();
  std::vector<double> MaxVel = m_params->GetMaxVel();
  std::vector<double> MaxAcc = m_params->GetMaxAcc();
  std::vector<double> Offsets = m_params->GetOffsets();
  std::vector<double> LowerLimits = m_params->GetLowerLimits();
  std::vector<double> UpperLimits = m_params->GetUpperLimits();

  std::cout << " D  O  F  :" << DOF << std::endl;
  m_status.resize(DOF);
  m_ModuleTypes.resize(DOF);
  m_version.resize(DOF);
  m_dios.resize(DOF);
  m_positions.resize(DOF);
  m_velocities.resize(DOF);

  std::cout << "=========================================================================== " << std::endl;
  std::cout << "PowerCubeCtrl:Init: Trying to initialize with the following parameters: " << std::endl;
  std::cout << "DOF: " << DOF << std::endl;
  std::cout << "CanModule: " << CanModule << std::endl;
  std::cout << "CanDevice: " << CanDevice << std::endl;
  std::cout << "CanBaudrate: " << CanBaudrate << std::endl;
  std::cout << "ModulIDs: ";
  for (int i = 0; i < DOF; i++)
  {
    std::cout << ModulIDs[i] << " ";
  }
  std::cout << std::endl;

  std::cout << std::endl << "maxVel: ";
  for (int i = 0; i < DOF; i++)
  {
    std::cout << MaxVel[i] << " ";
  }

  std::cout << std::endl << "maxAcc: ";
  for (int i = 0; i < DOF; i++)
  {
    std::cout << MaxAcc[i] << " ";
  }

  std::cout << std::endl << "upperLimits: ";
  for (int i = 0; i < DOF; i++)
  {
    std::cout << UpperLimits[i] << " ";
  }

  std::cout << std::endl << "lowerLimits: ";
  for (int i = 0; i < DOF; i++)
  {
    std::cout << LowerLimits[i] << " ";
  }

  std::cout << std::endl << "offsets: ";
  for (int i = 0; i < DOF; i++)
  {
    std::cout << Offsets[i] << " ";
  }

  std::cout << std::endl << "=========================================================================== " << std::endl;
  std::ostringstream InitStr;
  InitStr << CanModule << ":" << CanDevice << "," << CanBaudrate;
  std::cout << "initstring = " << InitStr.str().c_str() << std::endl;

  pthread_mutex_lock(&m_mutex);
  ret = PCube_openDevice(&m_DeviceHandle, InitStr.str().c_str());
  pthread_mutex_unlock(&m_mutex);
  if (ret != 0)
  {
    std::ostringstream errorMsg;
    errorMsg << "Could not open device " << CanDevice << ", m5api error code: " << ret;
    m_ErrorMessage = errorMsg.str();
    return false;
  }

  m_CANDeviceOpened = true;

  int max_tries = 3;
  for (int i = 0; i < DOF; i++)
  {
    for (int reset_try = 0; reset_try < max_tries; reset_try++)
    {
      pthread_mutex_lock(&m_mutex);
      ret = PCube_resetModule(m_DeviceHandle, ModulIDs.at(i));
      pthread_mutex_unlock(&m_mutex);

      if (ret == 0)
      {
        break;
      }
      else if ((ret != 0) && (reset_try == (max_tries - 1)))
      {
        std::ostringstream errorMsg;
        errorMsg << "Could not reset module " << ModulIDs.at(i) << " during init. Errorcode during reset: " << ret << " Try to init once more.";
        m_ErrorMessage = errorMsg.str();
        return false;
      }
      else
      {
        // little break
        usleep(1500000);
      }
    }
  }
  std::cout << "number of moduleIDs" << ModulIDs.size() << std::endl;

  pthread_mutex_lock(&m_mutex);
  int number_of_modules = PCube_getModuleCount(m_DeviceHandle);
  pthread_mutex_unlock(&m_mutex);
  std::cout << "found " << number_of_modules << " modules." << std::endl;

  for (int i = 0; i < DOF; i++)
  {
    unsigned long serNo;
    unsigned short verNo;
    unsigned long defConfig;
    std::vector<std::string> Module_Types;

    pthread_mutex_lock(&m_mutex);
    ret = PCube_getModuleSerialNo(m_DeviceHandle, ModulIDs[i], &serNo);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }

    pthread_mutex_lock(&m_mutex);
    ret = PCube_getModuleVersion(m_DeviceHandle, ModulIDs[i], &verNo);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not find Module with ID " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }
    else
    {
      m_version[i] = verNo;
    }

    float gear_ratio;
    pthread_mutex_lock(&m_mutex);
    ret = PCube_getDefGearRatio(m_DeviceHandle, ModulIDs[i], &gear_ratio);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not get Module type with ID " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }
    else
    {
      if (true)
      {
        std::cout << "gear ratio: " << gear_ratio << std::endl;
        // return false;
      }
    }

    unsigned char type;
    pthread_mutex_lock(&m_mutex);
    ret = PCube_getModuleType(m_DeviceHandle, ModulIDs[i], &type);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not get Module type with ID " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }
    else
    {
      if (type != TYPEID_MOD_ROTARY)
      {
        std::cout << "wrong module type configured. Type must be rotary axis. Use Windows configuration software to change type." << std::endl;
        return false;
      }
    }

    // the typ -if PW or PRL- can be distinguished by the typ of encoder.
    pthread_mutex_lock(&m_mutex);
    ret = PCube_getDefSetup(m_DeviceHandle, ModulIDs[i], &defConfig);
    pthread_mutex_unlock(&m_mutex);

    ROS_DEBUG("module type check: %li (std::dec)", defConfig);

    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Error on communication with module " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }

    // Firmware version 4634 of PRL modules replies ABSOULTE_FEEDBACK, firmware 4638 replies RESOLVER_FEEDBACK.
    // Both means the same: Module is PRL. PW modules have encoders (ENCODER_FEEDBACK, s.M5API), but this bit is not set
    // is DefConfig word.
    // For new firmware versions this needs to be evaluated.
    if (((defConfig & CONFIG_ABSOLUTE_FEEDBACK) == CONFIG_ABSOLUTE_FEEDBACK) ||
        ((defConfig & CONFIG_RESOLVER_FEEDBACK) == CONFIG_RESOLVER_FEEDBACK))
    {
      m_ModuleTypes[i] = "PRL";
      ROS_DEBUG("Module %i is from type: PRL", i);
    }
    else
    {
      m_ModuleTypes[i] = "PW";
      ROS_DEBUG("Module %i is from type: PW", i);
    }

    std::cout << "Found module " << std::dec << ModulIDs[i] << " Serial: " << serNo << " Version: " << std::hex << verNo << std::endl;
  }

  // modules should be initialized now
  m_pc_status = PC_CTRL_OK;
  m_Initialized = true;

  std::vector<std::string> errorMessages;
  PC_CTRL_STATUS status;

  // update status variables
  updateStates();

  // grep updated status
  getStatus(status, errorMessages);

  // homing dependant on moduletype and if already homed
  bool successful = false;
  successful = doHoming();
  if (!successful)
  {
    std::cout << "PowerCubeCtrl:Init: homing not successful, aborting ...\n";
    return false;
  }

  // All modules initialized successfully
  m_pc_status = PC_CTRL_OK;
  return true;
}

bool PowerCubeCtrl::Close()
{
  if (m_CANDeviceOpened)
  {
    m_Initialized = false;
    m_CANDeviceOpened = false;

    pthread_mutex_lock(&m_mutex);
    PCube_closeDevice(m_DeviceHandle);
    pthread_mutex_unlock(&m_mutex);

    return true;
  }

  else
  {
    return false;
  }
}

bool PowerCubeCtrl::MoveJointSpaceSync(const std::vector<double>& target_pos)
{
  PCTRL_CHECK_INITIALIZED();

  unsigned int DOF = m_params->GetDOF();
  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  std::vector<double> m_maxVel = m_params->GetMaxVel();
  std::vector<double> m_maxAcc = m_params->GetMaxAcc();

  std::cout << "Starting MoveJointSpaceSync(Jointd Angle) ... " << std::endl;

  // Questions about method: Felix.Geibel@gmx.de
  std::vector<double> acc(DOF);
  std::vector<double> vel(DOF);
  std::vector<double> posnow(DOF);
  std::vector<double> velnow(DOF);
  double TG = 0;

  try
  {
    // Calcute the joint which need longest time to the goal
    if (!getJointAngles(posnow))
    {
      return false;
    }

    if (!getJointVelocities(velnow))
    {
      return false;
    }

    std::vector<double> times(DOF);
    for (int i = 0; i < DOF; i++)
    {
      RampCommand rm(posnow[i], velnow[i], target_pos[i], m_maxAcc[i], m_maxVel[i]);
      times[i] = rm.getTotalTime();
    }

    // determine the joint index that has the greates value for time
    int furthest = 0;
    double max = times[0];
    for (int i = 1; i < DOF; i++)
    {
      if (times[i] > max)
      {
        max = times[i];
        furthest = i;
      }
    }

    RampCommand rm_furthest(posnow[furthest], velnow[furthest], target_pos[furthest], m_maxAcc[furthest], m_maxVel[furthest]);

    double T1 = rm_furthest.T1();
    double T2 = rm_furthest.T2();
    double T3 = rm_furthest.T3();
    // Sum of times:
    TG = T1 + T2 + T3;

    acc[furthest] = m_maxAcc[furthest];     //maximal acceleration for furthest
    vel[furthest] = m_maxVel[furthest];     //maximal speed for furthest

    for (int i = 0; i < DOF; i++)
    {
      if (i != furthest)
      {
        double a;
        double v;
        // a und v berechnen:
        RampCommand::calculateAV(           //TODO Berechnungsfehler! In dieser Methode wird die Geschw. und Beschl. falsch berechnet!
          posnow[i],
          velnow[i],
          target_pos[i],
          TG, T3,
          m_maxAcc[i],
          m_maxVel[i],
          a,
          v);

        acc[i] = a;
        vel[i] = v;
        //              printf("i_a: %f \n", acc[i]);
        //              printf("i_v: %f \n", vel[i]);
      }
    }
  }
  catch (...)
  {
    return false;
  }

  // Send motion commands to hardware
  std::ostringstream errorMsg;
  int ret = 0;
  bool ok = true;

  //PCube_setDeviceDebug(m_Dev, 1, 2, -1);
  for (int i = 0; i < DOF; i++)
  {
    if (fabs(posnow[i] - target_pos[i]) > 1e-3)
    {
      double v = fabs(vel[i]);
      double a = fabs(acc[i]);
      ret = PCube_moveRamp(m_DeviceHandle, ModulIDs[i], target_pos[i], v, a);
      if (ret != 0)
      {
        errorMsg << "Module " << ModulIDs[i] << " PCube_moveRamp returned " << ret << std::endl;
        m_ErrorMessage = errorMsg.str();
        ok = false;
      }
    }
  }

  ret = PCube_startMotionAll(m_DeviceHandle);
  if (ret != 0)
  {
    errorMsg << "PCube_startMotionAll returned " << ret << std::endl;
    m_ErrorMessage = errorMsg.str();
    ok = false;
  }

  return ok;
}

/*
 * \brief Move joints with calculated velocities
 *
 * Calculating positions and times by desired value of the cob_trajectory_controller
 * \param velocities Vector
 */
bool PowerCubeCtrl::MoveVel(const std::vector<double>& vel)
{
  PCTRL_CHECK_INITIALIZED();

  //== init var ==================================================

  unsigned int DOF = m_params->GetDOF();
  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  std::vector<double> velocities;
  velocities.resize(DOF);
  velocities = vel;  // temp var for velocity because of const reference of the function

  std::ostringstream errorMsg;  // temp error msg for being copied to m_errorMessage

  std::vector<std::string> errorMessages;  // temp error msg for call of getStatus()
  PC_CTRL_STATUS status;                   // PowerCube Ctrl status variable

  std::vector<float> delta_pos;  // traviling distance for next command
  std::vector<float> delta_pos_horizon;
  delta_pos.resize(DOF);
  delta_pos_horizon.resize(DOF);

  std::vector<float> target_pos;  // absolute target postion that is desired with this command
  std::vector<float> target_pos_horizon;

  target_pos.resize(DOF);
  target_pos_horizon.resize(DOF);
  float target_time;  // time in milliseconds
  float target_time_horizon = 0;

  float delta_t;  // time from the last moveVel cmd to now

  std::vector<double> LowerLimits = m_params->GetLowerLimits();
  std::vector<double> UpperLimits = m_params->GetUpperLimits();
  std::vector<double> maxVels = m_params->GetMaxVel();

  std::vector<double> Offsets = m_params->GetOffsets();

  int ret;    // temp return value holder
  float pos;  // temp position variable for PCube_move.. cmds

  //== calculate destination position ============================
  // needed for limit handling and MoveStep command

  delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();

  m_last_time_pub = ros::Time::now();

  std::vector<double> pos_temp;
  pos_temp.resize(DOF);

  for (unsigned int i = 0; i < DOF; i++)
  {
    // limit step time to 50msec
    // TODO: set value 0.05 as parameter
    if (delta_t >= 0.050)
    {
      target_time = 0.050;  // sec
    }
    else
    {
      target_time = delta_t;  // sec
    }

    // add horizon to time before calculation of target position, to influence both time and position at the same time
    target_time_horizon = target_time + (float)m_horizon;  // sec
    // calculate travel distance
    delta_pos_horizon[i] = target_time_horizon * velocities[i];
    delta_pos[i] = target_time * velocities[i];

    ROS_DEBUG("delta_pos[%i]: %f target_time: %f velocitiy[%i]: %f", i, delta_pos[i], target_time, i, velocities[i]);

    // calculate target position
    target_pos_horizon[i] = m_positions[i] + delta_pos_horizon[i] - Offsets[i];
    ROS_DEBUG("target_pos[%i]: %f m_position[%i]: %f", i, target_pos[i], i, m_positions[i]);
  }

  //== check input parameter =====================================

  if (velocities.size() != DOF)
  {
    m_ErrorMessage = "Skipping command: Commanded velocities and DOF are not same dimension.";
    return false;
  }

  for (unsigned int i = 0; i < DOF; i++)
  {
    if (velocities[i] > maxVels[i])
    {
      // set velocities command to max value
      velocities[i] = maxVels[i];

      // TODO: add ros_warn

      ROS_INFO("Velocity %f exceeds limit %f for axis %i. moving with max velocity %f instead", velocities[i], maxVels[i], i, maxVels[i]);
    }

    // TODO: add second limit "safty limit"
    // if target position is outer limits and the command velocity is in in direction away from working range, skip
    // command
    if ((target_pos[i] < LowerLimits[i] + Offsets[i]) && (velocities[i] < 0))
    {
      ROS_INFO("Skipping command: %f Target position exceeds lower limit (%f).", target_pos[i], LowerLimits[i]);
      // target position is set to actual position and velocity to Null. So only movement in the non limit direction is possible.

      pthread_mutex_lock(&m_mutex);
      PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
      pthread_mutex_unlock(&m_mutex);

      return true;
    }

    // if target position is outer limits and the command velocity is in in direction away from working range, skip command
    if ((target_pos[i] > UpperLimits[i] + Offsets[i]) && (velocities[i] > 0))
    {
      ROS_INFO("Skipping command: %f Target position exceeds upper limit (%f).", target_pos[i], UpperLimits[i]);
      // target position is set to actual position. So only movement in the non limit direction is possible.

      pthread_mutex_lock(&m_mutex);
      PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
      pthread_mutex_unlock(&m_mutex);

      return true;
    }
  }

  //== check system status ======================================

  getStatus(status, errorMessages);

  if ((status != PC_CTRL_OK))
  {
    m_ErrorMessage.assign("");
    for (unsigned int i = 0; i < DOF; i++)
    {
      m_ErrorMessage.append(errorMessages[i]);
    }
    ROS_INFO("Error during movement. Status: %i \n", status);
    return false;
  }

  //== send velocity cmd to modules ==============================

  // convert the time to int in [ms]
  unsigned short time4motion = (unsigned short)((target_time_horizon)*1000.0);

  for (unsigned int i = 0; i < DOF; i++)
  {
    // check module type sending command (PRL-Modules can be driven with moveStepExtended(), PW-Modules can only be driven with less safe moveVelExtended())
    if ((m_ModuleTypes.at(i) == "PRL") && (m_version[i] >= Ready4MoveStep) && !m_params->GetUseMoveVel())
    {
      // ROS_DEBUG("Modul_id = %i, ModuleType: %s, step=%f, time=%f", m_params->GetModuleID(i), m_ModuleTypes[i].c_str(), target_pos[i], target_time);

      // ROS_INFO("Modul_id = %i, ModuleType: %s, step=%f, time4motion=%i [ms], target_time=%f, horizon: %f", m_params->GetModuleID(i), m_ModuleTypes[i].c_str(), delta_pos[i], time4motion, target_time, m_horizon);

      pthread_mutex_lock(&m_mutex);
      ret = PCube_moveStepExtended(m_DeviceHandle, m_params->GetModuleID(i), target_pos_horizon[i], time4motion, &m_status[i], &m_dios[i], &pos);
      pthread_mutex_unlock(&m_mutex);
    }
    else  
    {
      pthread_mutex_lock(&m_mutex);
      ret = PCube_moveVelExtended(m_DeviceHandle, m_params->GetModuleID(i), velocities[i], &m_status[i], &m_dios[i], &pos);
      pthread_mutex_unlock(&m_mutex);
    }

    if (ret != 0)
    {
      ROS_DEBUG("Com Error: %i", ret);
      pos = m_positions[i];
      // m_pc_status = PC_CTRL_ERR;
      // TODO: add error msg for diagnostics if error occours often
    }

    // !!! Position in pos is position before moveStep movement, to get the expected position after the movement
    // (required as input to the next moveStep command) we add the delta position (cmd_pos) !!!
    m_positions[i] = (double)pos + delta_pos[i] + Offsets[i];

    pos_temp[i] = (double)pos;
    // ROS_INFO("After cmd (%X) :m_positions[%i] %f = pos: %f + delta_pos[%i]: %f",m_status[i], i, m_positions[i], pos, i, delta_pos[i]);
  }

  updateVelocities(pos_temp, delta_t);

  // std::cout << "vel_com: " << velocities[1] << " vel_hori: " << delta_pos_horizon[1]/target_time_horizon << " vel_real[1]: " << m_velocities.at(1) << std::endl;

  pthread_mutex_lock(&m_mutex);
  PCube_startMotionAll(m_DeviceHandle);
  pthread_mutex_unlock(&m_mutex);

  return true;
}

// Calculation of velocities based on vel = 1/(6*dt) * (-pos(t-3) - 3*pos(t-2) + 3*pos(t-1) + pos(t))
void PowerCubeCtrl::updateVelocities(std::vector<double> pos_temp, double delta_t)
{
  unsigned int DOF = m_params->GetDOF();

  if (m_cached_pos.empty())
  {
    std::vector<double> null;
    for (unsigned int i = 0; i < DOF; i++)
    {
      null.push_back(0);
    }

    m_cached_pos.push_back(null);
    m_cached_pos.push_back(null);
    m_cached_pos.push_back(null);
    m_cached_pos.push_back(null);
  }

  m_cached_pos.push_back(pos_temp);
  m_cached_pos.pop_front();

  std::vector<double> last_pos = m_cached_pos.front();

  for (unsigned int i = 0; i < DOF; i++)
  {
    m_velocities[i] = 1 / (6 * delta_t) * (-m_cached_pos[0][i] - (3 * m_cached_pos[1][i]) + (3 * m_cached_pos[2][i]) + m_cached_pos[3][i]);
    // m_velocities[i] = (m_cached_pos[3][i] - m_cached_pos[2][i])/delta_t;
    // m_velocities[i] = (pos_temp.at(i)-last_pos.at(i))/delta_t;
  }
}

bool PowerCubeCtrl::Stop()
{
  unsigned int DOF = m_params->GetDOF();
  std::vector<int> ModulIDs = m_params->GetModuleIDs();


  for (unsigned int i = 0; i < DOF; i++)
  {
    pthread_mutex_lock(&m_mutex);
    int ret = PCube_haltModule(m_DeviceHandle, ModulIDs.at(i));
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not reset all modules, m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }
  }

  usleep(500000);

  for (unsigned int i = 0; i < DOF; i++)
  {
    pthread_mutex_lock(&m_mutex);
    int ret = PCube_resetModule(m_DeviceHandle, ModulIDs.at(i));
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not reset all modules, m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }
  }
  return true;
}

bool PowerCubeCtrl::Recover()
{
  unsigned int DOF = m_params->GetDOF();
  std::vector<int> ModulIDs = m_params->GetModuleIDs();
  std::vector<double> MaxVel = m_params->GetMaxVel();
  std::vector<double> MaxAcc = m_params->GetMaxAcc();
  std::vector<double> Offsets = m_params->GetOffsets();

  std::vector<std::string> errorMessages;
  PC_CTRL_STATUS status;

  unsigned long state = PC_CTRL_OK;
  unsigned char dio;
  float position;
  int ret = 0;

  // check for each module if reset is necessary
  for (unsigned int i = 0; i < DOF; i++)
  {
    pthread_mutex_lock(&m_mutex);
    ret = getPositionAndStatus(i, &state, &dio, &position);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      m_pc_status = PC_CTRL_ERR;
      std::cout << "State: Error com with Module: " << i << " Time: " << ros::Time::now() << std::endl;
      return false;
    }

    // if module is in error state --> reset
    if (state & STATEID_MOD_ERROR)
    {
      pthread_mutex_lock(&m_mutex);
      ret = PCube_resetModule(m_DeviceHandle, m_params->GetModuleID(i));
      pthread_mutex_unlock(&m_mutex);
      if (ret != 0)
      {
        m_pc_status = PC_CTRL_ERR;
        std::cout << "State: Error com with Module: " << i << " Time: " << ros::Time::now() << std::endl;
        return false;
      }
    }
  }

  // time for reboot
  usleep(500000);

  // check is everything is ok now
  updateStates();

  if (m_pc_status == PC_CTRL_NOT_HOMED)
  {
    if (!doHoming())
    {
      return false;
    }
  }

  usleep(500000);

  // modules should be recovered now
  m_pc_status = PC_CTRL_OK;

  updateStates();
  // check if modules are really back to normal state
  getStatus(status, errorMessages);

  if ((status != PC_CTRL_OK))
  {
    m_ErrorMessage.assign("");
    for (int i = 0; i < m_params->GetDOF(); i++)
    {
      m_ErrorMessage.append(errorMessages[i]);
    }
    return false;
  }

  m_pc_status = PC_CTRL_OK;
  return true;
}

bool PowerCubeCtrl::setMaxVelocity(double maxVelocity)
{
  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  PCTRL_CHECK_INITIALIZED();
  for (int i = 0; i < m_params->GetDOF(); i++)
  {
    pthread_mutex_lock(&m_mutex);
    int ret = PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocity);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not set MaxVelocity in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }

    std::vector<double> maxVelocities(maxVelocity);
    m_params->SetMaxVel(maxVelocities);
  }

  return true;
}

bool PowerCubeCtrl::setMaxVelocity(const std::vector<double>& maxVelocities)
{
  PCTRL_CHECK_INITIALIZED();

  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  for (int i = 0; i < m_params->GetDOF(); i++)
  {
    pthread_mutex_lock(&m_mutex);
    // std::cout << "------------------------------> PCube_setMaxVel()" << std::endl;
    int ret = PCube_setMaxVel(m_DeviceHandle, m_params->GetModuleID(i), maxVelocities[i]);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not set MaxVelocity in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }

    m_params->SetMaxVel(maxVelocities);
  }

  return true;
}

bool PowerCubeCtrl::setMaxAcceleration(double maxAcceleration)
{
  PCTRL_CHECK_INITIALIZED();

  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  for (int i = 0; i < m_params->GetDOF(); i++)
  {
    pthread_mutex_lock(&m_mutex);
    // std::cout << "------------------------------> PCube_setMaxAcc()" << std::endl;
    int ret = PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAcceleration);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not set MaxAcceleration in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }

    std::vector<double> maxAccelerations(maxAcceleration);
    m_params->SetMaxAcc(maxAccelerations);
  }

  return true;
}

bool PowerCubeCtrl::setMaxAcceleration(const std::vector<double>& maxAccelerations)
{
  PCTRL_CHECK_INITIALIZED();

  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  for (int i = 0; i < m_params->GetDOF(); i++)
  {
    pthread_mutex_lock(&m_mutex);
    int ret = PCube_setMaxAcc(m_DeviceHandle, m_params->GetModuleID(i), maxAccelerations[i]);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::ostringstream errorMsg;
      errorMsg << "Could not set MaxAcceleration in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
      m_ErrorMessage = errorMsg.str();
      return false;
    }

    m_params->SetMaxAcc(maxAccelerations);
  }

  return true;
}

bool PowerCubeCtrl::setHorizon(double horizon)
{
  m_horizon = horizon;

  return true;
}

double PowerCubeCtrl::getHorizon()
{
  return m_horizon;
}

bool PowerCubeCtrl::setSyncMotion()
{
  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  if (m_CANDeviceOpened)
  {
    for (int i = 0; i < m_params->GetDOF(); i++)
    {
      unsigned long confword;

      pthread_mutex_lock(&m_mutex);
      PCube_getConfig(m_DeviceHandle, m_params->GetModuleID(i), &confword);
      pthread_mutex_unlock(&m_mutex);

      pthread_mutex_lock(&m_mutex);
      int ret = PCube_setConfig(m_DeviceHandle, m_params->GetModuleID(i), confword | CONFIGID_MOD_SYNC_MOTION);
      pthread_mutex_unlock(&m_mutex);
      if (ret != 0)
      {
        std::ostringstream errorMsg;
        errorMsg << "Could not set SyncMotion in Module ID: " << ModulIDs[i] << ", m5api error code: " << ret;
        m_ErrorMessage = errorMsg.str();
        return false;
      }
    }
    return true;
  }
  else
  {
    return false;
  }
}
bool PowerCubeCtrl::setASyncMotion()
{
  if (m_CANDeviceOpened)
  {
    for (int i = 0; i < m_params->GetDOF(); i++)
    {
      unsigned long confword;

      pthread_mutex_lock(&m_mutex);
      PCube_getConfig(m_DeviceHandle, m_params->GetModuleID(i), &confword);
      pthread_mutex_unlock(&m_mutex);

      pthread_mutex_lock(&m_mutex);
      PCube_setConfig(m_DeviceHandle, m_params->GetModuleID(i), confword & (~CONFIGID_MOD_SYNC_MOTION));
      pthread_mutex_unlock(&m_mutex);
    }
    return true;
  }

  else
  {
    return false;
  }
}
bool PowerCubeCtrl::updateStates()
{
  PCTRL_CHECK_INITIALIZED();

  unsigned int DOF = m_params->GetDOF();
  unsigned long state;
  PC_CTRL_STATUS pc_status = PC_CTRL_ERR;
  std::vector<std::string> ErrorMessages;
  std::vector<double> Offsets = m_params->GetOffsets();
  std::ostringstream errorMsg;

  unsigned char dio;
  float position;
  int ret = 0;

  for (unsigned int i = 0; i < DOF; i++)
  {
    state = m_status[i];
    pthread_mutex_lock(&m_mutex);
    ret = getPositionAndStatus(i, &state, &dio, &position);
    pthread_mutex_unlock(&m_mutex);

    if (ret != 0)
    {
      // m_pc_status = PC_CTRL_ERR;
      ROS_DEBUG("Error on com in UpdateStates");
      return true;
      // errorMsg << "State: Error com with Module [" <<  i << "]";
      // ErrorMessages[i] = errorMsg.str();
    }
    else
    {
      ROS_DEBUG("Module %i, State: %li, Time: %f", i, state, ros::Time::now().toSec());

      m_status[i] = state;
      m_dios[i] = dio;
      m_positions[i] = position + Offsets[i];
    }
  }

  /*
  double delta_t = ros::Time::now().toSec() - m_last_time_pub.toSec();
  m_last_time_pub = ros::Time::now();

  updateVelocities(m_positions, delta_t);
  */

  // evaluate state for translation for diagnostics msgs
  getStatus(pc_status, ErrorMessages);

  for (unsigned int i = 0; i < ErrorMessages.size(); i++)
  {
    m_ErrorMessage.clear();
    m_ErrorMessage.assign("");
    m_ErrorMessage.append(ErrorMessages[i]);
  }

  return true;
}

bool PowerCubeCtrl::getStatus(PC_CTRL_STATUS& status, std::vector<std::string>& errorMessages)
{
  unsigned int DOF = m_params->GetDOF();
  std::vector<int> ModuleIDs = m_params->GetModuleIDs();

  std::vector<PC_CTRL_STATUS> StatusArray;
  StatusArray.resize(DOF);

  errorMessages.clear();
  errorMessages.resize(DOF);

  status = PC_CTRL_OK;

  for (unsigned int i = 0; i < DOF; i++)
  {
    std::ostringstream errorMsg;

    if (m_status[i] & STATEID_MOD_POWERFAULT)
    {
      if (m_status[i] & STATEID_MOD_POW_VOLT_ERR)
      {
        errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
        errorMsg << "Motor voltage below minimum value! Check Emergency Stop!";
        errorMessages[i] = errorMsg.str();
      }
      else if (m_status[i] & STATEID_MOD_POW_FET_TEMP)
      {
        errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
        errorMsg << "Overheated power transitors! Power must be switched of to reset this error.";
        errorMessages[i] = errorMsg.str();
      }
      else if (m_status[i] & STATEID_MOD_POW_INTEGRALERR)
      {
        errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
        errorMsg << "The drive has been overloaded and the servo loop has been disabled. Power must be switched off to reset this error.";
        errorMessages[i] = errorMsg.str();
      }
      StatusArray[i] = PC_CTRL_POW_VOLT_ERR;
    }
    else if (m_status[i] & STATEID_MOD_TOW_ERROR)
    {
      errorMsg << "Error in Module " << ModuleIDs[i] << ": ";
      errorMsg << "The servo loop was not able to follow the target position!";
      errorMessages[i] = errorMsg.str();
      StatusArray[i] = PC_CTRL_ERR;
    }
    else if (m_status[i] & STATEID_MOD_ERROR)
    {
      // STOP the motion for each module
      pthread_mutex_lock(&m_mutex);
      PCube_haltModule(m_DeviceHandle, m_params->GetModuleID(i));
      pthread_mutex_unlock(&m_mutex);

      errorMsg << "Error in  Module " << ModuleIDs[i];
      errorMsg << " : Status code: " << std::hex << m_status[i];
      errorMessages[i] = errorMsg.str();
      StatusArray[i] = PC_CTRL_ERR;
    }
    else if (m_pc_status & PC_CTRL_ERR)
    {
      Stop();  // stop all motion
      errorMsg << "PowerCubeCtrl is in global error state";
      errorMessages[i] = errorMsg.str();
      StatusArray[i] = PC_CTRL_ERR;
    }
    else
    {
      errorMsg << "Module with Id " << ModuleIDs[i];
      errorMsg << ": Status OK.";
      errorMessages[i] = errorMsg.str();
      StatusArray[i] = PC_CTRL_OK;
    }
  }

  // search for worst status
  for (unsigned int i = 0; i < DOF; i++)
  {
    if ((int)StatusArray[i] <= (int)status)
    {
      status = StatusArray[i];
    }
  }

  m_pc_status = status;

  return true;
}

std::vector<unsigned long> PowerCubeCtrl::getVersion()
{
  return m_version;
}
bool PowerCubeCtrl::statusMoving()
{
  PCTRL_CHECK_INITIALIZED();

  for (int i = 0; i < m_params->GetDOF(); i++)
  {
    if (m_status[i] & STATEID_MOD_MOTION)
      return true;
  }
  return false;
}

std::vector<double> PowerCubeCtrl::getPositions()
{
  return m_positions;
}

std::vector<double> PowerCubeCtrl::getVelocities()
{
  return m_velocities;
}

std::vector<double> PowerCubeCtrl::getAccelerations()
{
  return m_accelerations;
}

bool PowerCubeCtrl::getJointAngles(std::vector<double>& result)
{
  PCTRL_CHECK_INITIALIZED();

  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  int ret = 0;
  float pos;

  for (int i = 0; i < m_params->GetDOF(); i++)
  {
    pthread_mutex_lock(&m_mutex);
    ret = PCube_getPos(m_DeviceHandle, ModulIDs[i], &pos);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::cout << "PCubeCtrl::getConfig(): getPos(" << ModulIDs[i] << ") return " << ret << std::endl;
      return false;
    }
    result[i] = pos;
  }

  return true;
}

bool PowerCubeCtrl::getJointVelocities(std::vector<double>& result)
{
  PCTRL_CHECK_INITIALIZED();

  std::vector<int> ModulIDs = m_params->GetModuleIDs();

  int ret = 0;
  float vel;

  for (int i = 0; i < m_params->GetDOF(); i++)
  {
    pthread_mutex_lock(&m_mutex);
    ret = PCube_getVel(m_DeviceHandle, ModulIDs[i], &vel);
    pthread_mutex_unlock(&m_mutex);
    if (ret != 0)
    {
      std::cout << "PCubeCtrl::getConfig(): getVel(" << ModulIDs[i] << ") return " << ret << std::endl;
      return false;
    }
    result[i] = vel;
  }

  return true;
}

bool PowerCubeCtrl::doHoming()
{
  unsigned int DOF = m_params->GetDOF();
  std::vector<int> ModuleIDs = m_params->GetModuleIDs();
  std::vector<double> LowerLimits = m_params->GetLowerLimits();
  std::vector<double> UpperLimits = m_params->GetUpperLimits();

  std::vector<double> Offsets = m_params->GetOffsets();

  double max_homing_time = 15.0;  // seconds
  double homing_time = 999.0;     // set to 0 if any module is homed
  double intervall = 0.1;

  unsigned long state = PC_CTRL_ERR;
  unsigned char dio;
  float position;
  int ret = 0;

  for (unsigned int i = 0; i < DOF; i++)
  {
    pthread_mutex_lock(&m_mutex);
    ret = getPositionAndStatus(i, &state, &dio, &position);
    pthread_mutex_unlock(&m_mutex);

    // check and init m_position variable for trajectory controller
    if ((position > UpperLimits[i] + Offsets[i]) || (position < LowerLimits[i] + Offsets[i]))
    {
      std::ostringstream errorMsg;
      errorMsg << "Module " << ModuleIDs[i] << " has position " << position << " that is outside limits ("                << UpperLimits[i] + Offsets[i] << " <-> " << LowerLimits[i] + Offsets[i] << std::endl;
      if ((m_ModuleTypes.at(i) == "PW") || (m_ModuleTypes.at(i) == "other"))
      {
        std::cout << "Position error for PW-Module. Init is aborted. Try to reboot the robot." << std::endl;
        m_ErrorMessage = errorMsg.str();
        m_pc_status = PC_CTRL_ERR;
        return false;
      }
      else if (m_ModuleTypes.at(i) == "PRL")
      {
        ROS_INFO("Position of Module %i is outside limits. Module can only be moved in opposite direction.", i);
        ROS_INFO("Homing for Module: %i not necessary", m_params->GetModuleID(i));
      }
      else
      {
        ROS_INFO("Module type incorrect. (in func. PowerCubeCtrl::doHoming();)");
        return false;
      }
    }
    else
    {
      m_positions[i] = position + Offsets[i];
    }

    // check module type before homing (PRL-Modules need not to be homed by ROS)
    if ((m_ModuleTypes.at(i) == "PW") || (m_ModuleTypes.at(i) == "other"))
    {
      pthread_mutex_lock(&m_mutex);
      ret = getPositionAndStatus(i, &state, &dio, &position);
      pthread_mutex_unlock(&m_mutex);

      if (ret != 0)
      {
        ROS_INFO("Error on communication with Module: %i.", m_params->GetModuleID(i));
        m_pc_status = PC_CTRL_ERR;
        return false;
      }

      // only do homing when necessary
      if (!(state & STATEID_MOD_HOME))
      {
        // homing timer
        homing_time = 0.0;

        pthread_mutex_lock(&m_mutex);
        ret = PCube_homeModule(m_DeviceHandle, ModuleIDs[i]);
        pthread_mutex_unlock(&m_mutex);

        ROS_INFO("Homing started at: %f", ros::Time::now().toSec());

        if (ret != 0)
        {
          ROS_INFO("Error while sending homing command to Module: %i. I try to reset the module.", i);

          // reset module with the hope that homing works afterwards
          pthread_mutex_lock(&m_mutex);
          ret = PCube_resetModule(m_DeviceHandle, ModuleIDs[i]);
          pthread_mutex_unlock(&m_mutex);
          if (ret != 0)
          {
            std::ostringstream errorMsg;
            errorMsg << "Can't reset module after homing error" << ModuleIDs[i] << ", m5api error code: " << ret;
            m_ErrorMessage = errorMsg.str();
          }

          // little break for reboot
          usleep(200000);

          pthread_mutex_lock(&m_mutex);
          ret = PCube_homeModule(m_DeviceHandle, ModuleIDs[i]);
          pthread_mutex_unlock(&m_mutex);
          if (ret != 0)
          {
            std::ostringstream errorMsg;
            errorMsg << "Can't start homing for module " << ModuleIDs[i] << ", tried reset with no success, m5api error code: " << ret;
            m_ErrorMessage = errorMsg.str();
            return false;
          }
        }
      }
      else
      {
        ROS_INFO("Homing for Module: %i not necessary", m_params->GetModuleID(i));
      }
    }
    else
    {
      ROS_INFO("Homing for PRL-Module %i not necessary", m_params->GetModuleID(i));
    }
  }

  for (unsigned int i = 0; i < DOF; i++)
  {
    unsigned long int help;
    do
    {
      pthread_mutex_lock(&m_mutex);
      PCube_getModuleState(m_DeviceHandle, ModuleIDs[i], &help);
      pthread_mutex_unlock(&m_mutex);
      ROS_DEBUG("Homing active for Module: %i State: %li", ModuleIDs.at(i), help);

      usleep(intervall * 1000000);  // convert sec to usec
      homing_time += intervall;
      if (homing_time >= max_homing_time)
      {
        Stop();
        break;
      }

    } while ((help & STATEID_MOD_HOME) == 0);

    m_status[i] = help;
    ROS_DEBUG("State of Module %i : %li", ModuleIDs.at(i), help);
  }

  for (unsigned int i = 0; i < DOF; i++)
  {
    if (!(m_status[i] & STATEID_MOD_HOME) || (m_status[i] & STATEID_MOD_ERROR))
    {
      std::cout << "Homing failed: Error in  Module " << ModuleIDs[i] << std::endl;
      m_pc_status = PC_CTRL_NOT_HOMED;
      return false;
    }

    ROS_INFO("Homing for Modul %i done.", ModuleIDs.at(i));
  }

  // modules successfully homed
  m_pc_status = PC_CTRL_OK;
  return true;
}

bool PowerCubeCtrl::getPositionAndStatus(int i, unsigned long* state, unsigned char* dio, float* position)
{
  int ret = 0;

  if (m_version[i] > VERSION_ELECTR3_FIRST or
      (m_version[i] > VERSION_ELECTR2_FIRST and m_version[i] < VERSION_ELECTR2_LAST))
  {
    ret = PCube_getStateDioPos(m_DeviceHandle, m_params->GetModuleID(i), state, dio, position);
  }
  else
  {
    ret = PCube_getPos(m_DeviceHandle, m_params->GetModuleID(i), position);
    ret |= PCube_getModuleState(m_DeviceHandle, m_params->GetModuleID(i), state);
    unsigned long puiValue;
    ret |= PCube_getDioData(m_DeviceHandle, m_params->GetModuleID(i), &puiValue);
    *dio = (unsigned char)puiValue;
  }

  return ret;
}

bool m_TranslateError2Diagnosics(std::ostringstream* errorMsg)
{
  return true;
}