simulatedArm.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.
 */


#include <schunk_powercube_chain/simulatedArm.h>
#include <schunk_powercube_chain/simulatedMotor.h>
#include <math.h>

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

simulatedArm::simulatedArm()
{
  std::cerr << "==============Starting Simulated Powercubes\n";
  m_DOF = 0;
  m_Initialized = false;
  m_motors.clear();
}

simulatedArm::~simulatedArm()
{
  // nothing to do...
  ;
}

bool simulatedArm::Init(PowerCubeCtrlParams* params)
{
  m_DOF = params->GetNumberOfDOF();
  ;

  double vmax = 1.0;
  double amax = 1.5;

  setMaxVelocity(vmax);
  setMaxAcceleration(amax);
  m_maxVel.resize(m_DOF);
  m_maxAcc.resize(m_DOF);

  std::vector<double> ul(m_DOF);
  std::vector<double> ll(m_DOF);
  ul = params->GetUpperLimits();
  ll = params->GetLowerLimits();
  m_maxVel = params->GetMaxVel();
  m_maxAcc = params->GetMaxAcc();

  for (int i = 0; i < m_DOF; i++)
  {
    m_motors.push_back(simulatedMotor(ll[i], ul[i], amax, vmax));
  }
  std::cerr << "===========Initializing Simulated Powercubes\n";
  m_Initialized = true;
  return true;
}

bool simulatedArm::Stop()
{
  for (int i = 0; i < m_DOF; i++)
    m_motors[i].stop();
  return true;
}

bool simulatedArm::MoveJointSpaceSync(const std::vector<double>& target)
{
  std::cerr << "======================TUTUTUTUT\n";
  PSIM_CHECK_INITIALIZED();

  std::vector<double> acc(m_DOF);
  std::vector<double> vel(m_DOF);

  double TG = 0;

  try
  {
    // Ermittle Joint, der bei max Geschw. und Beschl. am längsten braucht:

    std::vector<double> posnow;
    posnow.resize(m_DOF);
    if (getConfig(posnow) == false)
      return false;

    std::vector<double> velnow;
    velnow.resize(m_DOF);
    if (getJointVelocities(velnow) == false)
      return false;

    std::vector<double> times(m_DOF);

    for (int i = 0; i < m_DOF; i++)
    {
      RampCommand rm(posnow[i], velnow[i], target[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 < m_DOF; i++)
    {
      if (times[i] > max)
      {
        max = times[i];
        furthest = i;
      }
    }

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

    double T1 = rm_furthest.T1();
    double T2 = rm_furthest.T2();
    double T3 = rm_furthest.T3();

    // Gesamtzeit:
    TG = T1 + T2 + T3;

    // Jetzt Geschwindigkeiten und Beschl. für alle:
    acc[furthest] = m_maxAcc[furthest];
    vel[furthest] = m_maxVel[furthest];

    for (int i = 0; i < m_DOF; i++)
    {
      if (i != furthest)
      {
        double a;
        double v;
        // a und v berechnen:
        RampCommand::calculateAV(posnow[i], velnow[i], target[i], TG, T3, m_maxAcc[i], m_maxVel[i], a, v);

        acc[i] = a;
        vel[i] = v;
      }
    }
  }
  catch (...)
  {
    m_ErrorMessage.assign("Problem during calculation of a and av.");
    return false;
  }

  // Jetzt Bewegung starten:
  for (int i = 0; i < m_DOF; i++)
  {
    std::cout << "moving motor " << i << ": " << target[i] << ": " << vel[i] << ": " << acc[i] << "\n";
    m_motors[i].moveRamp(target[i], vel[i], acc[i]);
  }

  return true;
}

bool simulatedArm::MoveVel(const std::vector<double>& Vel)
{
  PSIM_CHECK_INITIALIZED();

  //    std::cerr << ".";
  //    std::cerr << "Vels: ";
  for (int i = 0; i < m_DOF; i++)
  {
    //          std::cerr << Vel[i] << " ";
    m_motors[i].moveVel(Vel[i]);
  }
  //    std::cerr << "\n";
  return true;
}

bool simulatedArm::MovePos(const std::vector<double>& Pos)
{
  PSIM_CHECK_INITIALIZED();

  for (int i = 0; i < m_DOF; i++)
    m_motors[i].movePos(Pos[i]);

  return true;
}

// Funktionen zum setzen von Parametern: //

bool simulatedArm::setMaxVelocity(double radpersec)
{
  PSIM_CHECK_INITIALIZED();

  m_maxVel.resize(m_DOF);

  for (int i = 0; i < m_DOF; i++)
  {
    m_maxAcc[i] = radpersec;
    m_motors[i].setMaxVelocity(radpersec);
  }

  return true;
}

bool simulatedArm::setMaxVelocity(const std::vector<double>& radpersec)
{
  PSIM_CHECK_INITIALIZED();

  m_maxAcc = radpersec;

  for (int i = 0; i < m_DOF; i++)
    m_motors[i].setMaxVelocity(radpersec[i]);

  return true;
}

bool simulatedArm::setMaxAcceleration(double radPerSecSquared)
{
  PSIM_CHECK_INITIALIZED();

  for (int i = 0; i < m_DOF; i++)
    m_motors[i].setMaxAcceleration(radPerSecSquared);

  return true;
}

bool simulatedArm::setMaxAcceleration(const std::vector<double>& radPerSecSquared)
{
  PSIM_CHECK_INITIALIZED();

  for (int i = 0; i < m_DOF; i++)
    m_motors[i].setMaxAcceleration(radPerSecSquared[i]);

  return true;
}

// hier die Funktionen zur Statusabfrage: //

bool simulatedArm::getConfig(std::vector<double>& result)
{
  PSIM_CHECK_INITIALIZED();

  result.resize(m_DOF);
  for (int i = 0; i < m_DOF; i++)
  {
    result[i] = m_motors[i].getAngle();
  }

  return true;
}

bool simulatedArm::getJointVelocities(std::vector<double>& result)
{
  PSIM_CHECK_INITIALIZED();

  result.resize(m_DOF);
  for (int i = 0; i < m_DOF; i++)
  {
    result[i] = m_motors[i].getVelocity();
  }

  return true;
}

bool simulatedArm::statusMoving()
{
  PSIM_CHECK_INITIALIZED();

  for (int i = 0; i < m_DOF; i++)
  {
    if (m_motors[i].statusMoving())
      return true;
  }
  return false;
}

bool simulatedArm::statusDec()
{
  PSIM_CHECK_INITIALIZED();

  for (int i = 0; i < m_DOF; i++)
  {
    if (m_motors[i].statusDec())
      return true;
  }
  return false;
}

bool simulatedArm::statusAcc()
{
  PSIM_CHECK_INITIALIZED();

  for (int i = 0; i < m_DOF; i++)
  {
    if (m_motors[i].statusAcc())
      return true;
  }
  return false;
}