motion.hpp

Go to the documentation of this file.

#ifndef QTCTRL_MOTION
#define QTCTRL_MOTION

#include <iostream>
#include <string.h>  // To get memcpy definition


class MotionModel {

  enum Limits {  
    vMin,    
    vMax,    
    omMax,   
    aMin,    
    aMax,    
    gmMin,   
    gmMax,   
    nbLimits,    
  };
  
  double limits[nbLimits]; 

public:
  MotionModel(const double& max_v, const double& max_omega,
              const double& max_a, const double& max_gamma)
    : MotionModel(-max_v, max_v, max_omega,
                  -max_a, max_a, -max_gamma, max_gamma) {}
  
  MotionModel(const double& min_v, const double& max_v, 
              const double& max_omega,
              const double& min_a, const double& max_a, 
              const double& min_gamma, const double& max_gamma) {
    double values[nbLimits] = { min_v, max_v, max_omega,
                                min_a, max_a, min_gamma, max_gamma };
    set(values);
  } // end of MotionModel(nbLimits x const double&)

  const double& minTranslVel() const { return limits[vMin]; }

  const double& maxTranslVel() const { return limits[vMax]; }

  const double& maxRotVel()    const { return limits[omMax]; }

  const double& minTranslAcc() const { return limits[aMin]; }

  const double& maxTranslAcc() const { return limits[aMax]; }

  const double& minRotAcc()    const { return limits[gmMin]; }

  const double& maxRotAcc()    const { return limits[gmMax]; }

  double limAcc(const double& lim_dist, const double& dist,
                const double& vel, const double& ldr_vel,
                const double& time_step) const;

  void applyAccelerationFactors(double& trans_vel, double& rot_vel,
                                const double& trans_fact,
                                const double& rot_fact, 
                                const double& time_step) const;

  void applyAccelerations(double& trans_vel, double& rot_vel,
                          const double& trans_acc,
                          const double& rot_acc, 
                          const double& time_step) const;
  
  void set(const double values[nbLimits]) 
  { memcpy(limits, values, sizeof(double) * nbLimits); }
  
}; // end of class MotionModel

#endif // QTCTRL_MOTION