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


// standard includes
#include <math.h>
#include <stddef.h>

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

// ToDo : ? Funktion
RampCommand::RampCommand(double x0, double v0, double xtarget, double amax, double vmax)
  : moveCommand(), m_x0(x0), m_v0(v0), m_xtarget(xtarget), m_amax(amax), m_vmax(vmax)
{
  m_umkehr = false;
  m_nachumkehr = NULL;
  m_vmax = fabs(m_vmax);
  m_amax = fabs(m_amax);
  double delta = m_xtarget - m_x0;
  if (delta < 0)
  {
    m_vmax = -m_vmax;
    m_amax = -m_amax;
  }

  if (m_v0 * m_vmax >= 0)
  {
    if (fabs(m_vmax) >= fabs(m_v0))
    {
      double delta1 = (m_vmax * m_vmax - 0.5 * m_v0 * m_v0) / m_amax;
      double delta2 = 0.5 * m_v0 * m_v0 / m_amax;

      if (fabs(delta) >= fabs(delta1))
      {
        m_T1 = (m_vmax - m_v0) / m_amax;
        m_a1 = m_amax;
        m_T3 = m_vmax / m_amax;
        m_a3 = -m_amax;
        m_T2 = (delta - delta1) / m_vmax;
        m_v2 = m_vmax;
      }
      else if (fabs(delta) >= fabs(delta2))
      {
        m_v2 = sqrt(delta * m_amax + 0.5 * m_v0 * m_v0);
        m_v2 = (delta < 0) ? -m_v2 : m_v2;
        m_T1 = (m_v2 - m_v0) / m_amax;
        m_a1 = m_amax;
        m_T2 = 0.0;
        m_T3 = m_v2 / m_amax;
        m_a3 = -m_amax;
      }
      else if (fabs(delta) > 0)
      {
        m_v2 = -sqrt(0.5 * m_v0 * m_v0 - m_amax * delta);
        m_v2 = (delta < 0) ? -m_v2 : m_v2;
        m_T2 = 0.0;
        m_T1 = (m_v0 - m_v2) / m_amax;
        m_a1 = -m_amax;
        m_T3 = -m_v2 / m_amax;
        m_a3 = m_amax;
      }
      else
      {
        m_T1 = 0;
        m_T2 = 0;
        m_T3 = 0;
        m_a1 = 0;
        m_v2 = 0;
        m_a3 = 0;
      }
    }
    else
    {
      double delta1 = 0.5 * m_v0 * m_v0 / m_amax;
      double vstern = m_v0 * 0.707106781;

      if (fabs(delta) >= fabs(delta1))
      {
        m_T1 = (m_v0 - m_vmax) / m_amax;
        m_a1 = -m_amax;
        m_T3 = m_vmax / m_amax;
        m_a3 = -m_amax;
        m_T2 = (delta - delta1) / m_vmax;
        m_v2 = m_vmax;
      }
      else if (fabs(m_vmax) >= fabs(vstern))
      {
        m_v2 = -sqrt(0.5 * m_v0 * m_v0 - m_amax * delta);
        m_v2 = (delta < 0) ? -m_v2 : m_v2;
        m_T2 = 0.0;
        m_T1 = (m_v0 - m_v2) / m_amax;
        m_a1 = -m_amax;
        m_T3 = -m_v2 / m_amax;
        m_a3 = m_amax;
      }
      else
      {
        m_T1 = (m_v0 + m_vmax) / m_amax;
        m_a1 = -m_amax;
        m_T2 = (delta - (0.5 * m_v0 * m_v0 / m_amax - m_vmax * m_vmax / m_amax)) / m_vmax;
        m_v2 = -m_vmax;
        m_T3 = m_vmax / m_amax;
        m_a3 = m_amax;
      }
    }
  }
  else
  {
    m_a1 = m_amax;
    m_T1 = -m_v0 / m_amax;
    m_umkehr = true;
    // RampCommand::RampCommand(double x0, double v0, double xtarget, double amax, double vmax)
    m_nachumkehr = new RampCommand(m_x0 + 0.5 * m_v0 * m_T1, 0.0, m_xtarget, m_amax, m_vmax);
    m_T2 = 0;
    m_T3 = 0;
    m_v2 = 0;
    m_a3 = 0;
  }
}

RampCommand::RampCommand(const RampCommand& rc)
{
  m_x0 = rc.m_x0;
  m_v0 = rc.m_v0;
  m_xtarget = rc.m_xtarget;
  m_amax = rc.m_amax;
  m_vmax = rc.m_vmax;

  m_T1 = rc.m_T1;
  m_T2 = rc.m_T2;
  m_T3 = rc.m_T3;
  m_a1 = rc.m_a1;
  m_v2 = rc.m_v2;
  m_a3 = rc.m_a3;

  m_umkehr = rc.m_umkehr;
  if (rc.m_umkehr)
    m_nachumkehr = new RampCommand(*rc.m_nachumkehr);
  else
    m_nachumkehr = NULL;
}

RampCommand& RampCommand::operator=(const RampCommand& rc)
{
  if (m_nachumkehr)
    delete m_nachumkehr;

  m_x0 = rc.m_x0;
  m_v0 = rc.m_v0;
  m_xtarget = rc.m_xtarget;
  m_amax = rc.m_amax;
  m_vmax = rc.m_vmax;

  m_T1 = rc.m_T1;
  m_T2 = rc.m_T2;
  m_T3 = rc.m_T3;
  m_a1 = rc.m_a1;
  m_v2 = rc.m_v2;
  m_a3 = rc.m_a3;

  m_umkehr = rc.m_umkehr;
  if (rc.m_umkehr)
    m_nachumkehr = new RampCommand(*rc.m_nachumkehr);
  else
    m_nachumkehr = NULL;

  return *this;
}

double RampCommand::getPos(double TimeElapsed)
{
  if (m_umkehr)
  {
    if (TimeElapsed <= m_T1)
    {
      return m_x0 + m_v0 * TimeElapsed + 0.5 * m_a1 * TimeElapsed * TimeElapsed;  // x = x0 + v0t + a/2*t^2
    }
    else
    {
      // std::cout << "in getPos(double)\n";
      return m_nachumkehr->getPos(TimeElapsed - m_T1);
    }
  }
  else
  {
    if (TimeElapsed <= m_T1)
    {
      return m_x0 + m_v0 * TimeElapsed + 0.5 * m_a1 * TimeElapsed * TimeElapsed;  // x = x0 + v0t + a/2*t^2
    }
    else if (TimeElapsed <= m_T1 + m_T2)
    {
      return m_x0 + m_v0 * m_T1 + 0.5 * m_a1 * m_T1 * m_T1 + m_v2 * (TimeElapsed - m_T1);  // x = x1 + v2*t
    }
    else if (TimeElapsed <= m_T1 + m_T2 + m_T3)
    {
      return m_x0 + m_v0 * m_T1 + 0.5 * m_a1 * m_T1 * m_T1 + m_v2 * m_T2 + m_v2 * (TimeElapsed - m_T1 - m_T2) +
             0.5 * m_a3 * (TimeElapsed - m_T1 - m_T2) * (TimeElapsed - m_T1 - m_T2);
      // x = x2 + v2t + a/2*t^2
    }
    else
      return m_xtarget;
  }
}

double RampCommand::getVel(double TimeElapsed)
{
  if (m_umkehr)
  {
    if (TimeElapsed <= m_T1)
    {
      return m_v0 + m_a1 * TimeElapsed;
    }
    else
    {
      return m_nachumkehr->getVel(TimeElapsed - m_T1);
    }
  }
  else
  {
    if (TimeElapsed <= m_T1)
    {
      return m_v0 + m_a1 * TimeElapsed;
    }
    else if (TimeElapsed <= m_T1 + m_T2)
    {
      return m_v2;
    }
    else if (TimeElapsed <= m_T1 + m_T2 + m_T3)
    {
      return m_v2 + m_a3 * (TimeElapsed - m_T1 - m_T2);
    }
    else
      return 0.0;
  }
}

double RampCommand::getTotalTime()
{
  // std::cout << "getTotalTime()\n";
  if (m_umkehr)
    return m_T1 + m_nachumkehr->getTotalTime();
  else
    return m_T1 + m_T2 + m_T3;
}

void RampCommand::calculateAV(double x0, double v0, double xtarget, double time, double T3, double amax, double vmax,
                              double& acc, double& vel)
{
  // std::ostream& out(debug);

  double TG = time;

  if (TG <= 0)
  {
    acc = 0;
    vel = 0;
  }
  else
  {
    double delta = xtarget - x0;
    amax = fabs(amax);
    amax = (delta >= 0) ? amax : -amax;

    double d1 = fabs(v0) * TG - 0.5 * T3 * fabs(v0);
    double d2h = (v0 > 0) ? -T3 * v0 + v0 * sqrt(2 * T3 * TG) : -T3 * v0 - v0 * sqrt(2 * T3 * TG);
    double d2l = (v0 > 0) ? -T3 * v0 - v0 * sqrt(2 * T3 * TG) : -T3 * v0 + v0 * sqrt(2 * T3 * TG);
    // double d2 = 0.5 * TG * v0;
    double d3 = 0.5 * v0 * v0 / amax;
    // out << "d1:\t" << d1 << "\n";
    // out << "d2h:\t" << d2h << "\n";
    // out << "d2l:\t" << d2l << "\n";
    // out << "d3:\t" << d3 << "\n";
    // out << "delta:\t" << delta << "\n";

    if (T3 > 0)
    {
      if (fabs(delta) >= d1)
      {
        /* out << "  Case 1\n"; */
        /*           ----------         */
        /*      /          \    */
        /*                  \   */

        double a = (TG / T3 - 1.0);
        double b = v0 - delta / T3;
        double c = -0.5 * v0 * v0;
        // out << "  a=" << a << " | b=" << b << " | c=" << c << endl;
        if (delta >= 0)
          vel = (-b + sqrt(b * b - 4.0 * a * c)) / (2.0 * a);
        else
          vel = (-b - sqrt(b * b - 4.0 * a * c)) / (2.0 * a);

        acc = vel / T3;
      }
      else if (delta >= d2h || delta <= d2l)
      {
        /* out << "  Case 2\n"; */
        /*          \                           */
        /*       ----------             */
        /*                 \    */
        double a = TG;
        double b = -delta - T3 * v0;
        double c = 0.5 * v0 * v0 * T3;
        // out << "  a=" << a << " | b=" << b << " | c=" << c << endl;
        if (delta >= 0)
          vel = (-b + sqrt(b * b - 4.0 * a * c)) / (2.0 * a);
        else
          vel = (-b - sqrt(b * b - 4.0 * a * c)) / (2.0 * a);

        acc = vel / T3;
      }
      else if (fabs(delta) >= d3)
      {


        acc = amax;

        if (delta * v0 > 0)
          vel = (2 * acc * delta - v0 * v0) / (2 * acc * TG - 2 * v0);
        else if (-4 * acc * delta + acc * acc * TG * TG + 2 * acc * TG * v0 - v0 * v0 >= 0)
          if (delta > 0)
          {
            // delta>0 (-> a>0), v0 < 0:
            vel = 0.5 * (acc * TG + v0 - sqrt(-4 * acc * delta + acc * acc * TG * TG + 2 * acc * TG * v0 - v0 * v0));
          }
          else
          {
            // delta<0 (-> a<0), v0 > 0:
            vel = 0.5 * (acc * TG + v0 + sqrt(-4 * acc * delta + acc * acc * TG * TG + 2 * acc * TG * v0 - v0 * v0));
          }
        else
        {

          vel = vmax;
        }
      }
      else
      {

        if (4 * delta * delta - 4 * delta * TG * v0 + 2 * TG * TG * v0 * v0 >= 0)
          if (delta * v0 > 0)
            acc = (-2.0 * delta + TG * v0 + sqrt(4 * delta * delta - 4 * delta * TG * v0 + 2 * TG * TG * v0 * v0)) / (TG * TG);
          else
            acc = (-2.0 * delta + TG * v0 - sqrt(4 * delta * delta - 4 * delta * TG * v0 + 2 * TG * TG * v0 * v0)) / (TG * TG);
        else
          acc = amax;

        vel = vmax;
      }
    }
    else if (T3 == 0)
    {
      acc = amax;
      vel = vmax;
    }
    else
    {
      acc = 0;
      vel = 0;
    }
  }
}