moveCommand.cpp

Go to the documentation of this file.

// 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;
                }
        }
}