bhspeedprofiler.h

/***************************************************************************
                          bhspeedprofiler.h
                             -------------------
 one-dimensional continuous filter with a-posteriori discretization, based
 on the paper "trajectory generation as a non-linear filter" by john lloyd,
 corrected and adapted by torsten scherer.

    copyright            : (c) 2004 Tim Baier
    email                : tbaier@informatik.uni-hamburg.de
 ***************************************************************************/

#ifndef BHSPEEDPROFILER_H
#define BHSPEEDPROFILER_H

#include <iostream>
#include <cmath>

class BHSpeedProfiler
{

public:

  BHSpeedProfiler (void)
  {
    // we need to be able to change parameters later anyway, so there's no
    // point in having a full-fledged constructor if there's also a function to
    // change all this...
    URate = -1.0;                             // ok, at least mark it as illegal
  };

  virtual ~BHSpeedProfiler (void){};

  void SetParameters (const double newRate, const double a, const double vm);
  void SetPositionAndVelocity (const double startPos, const double v0);
  void SetTarget (const double endPos);

  void SetTrajectory (const double startPos, const double endPos);
  
  // compute deviation of point `p' from trajectory and/or target

  double ComputeTrajectoryError (const double pos);
  double ComputeTargetError (const double pos);

  // declaring these to return `const CVEC &' should prohibit writing to them

  double GetTarget (void) const;
  double GetTime (void) const;
  double GetNewPosition (void) const;
  double GetNewVelocity (void) const;

  //

  virtual double ComputeProfile (void) = 0;
  virtual bool IntegrateProfile (void) = 0;

  virtual double StretchProfile (const double tMin);


protected:

  // generic profile computation, doesn't in fact touch `this'...

  double computeProfile (const double startPos,
                         const double startSpeed,
                         const double endPos,
                         double acce,
                         const double vmax,
                         double &timeStepInOut,
                         double &speedOut) const;

  // generic profile integration, doesn't in fact touch `this'...

  bool integrateProfile (const double x0In,
                         const double v0In,
                         const double xeIn,
                         const double tsIn,
                         const double vpIn,
                         const double a,
                         const double te,
                         double &x1Out,
                         double &v1Out) const;

  // generic information

  double URate, Accel, VMax;

  // world coordinates

  double StartPos, StartVel, EndPos;
  double NextPos, NextVel;   // next setpoint

  // times and peak velocities are always needed. for CFILT_ORTHO there is no
  // real projection, so the names are a bit wrong, but what the heck...

  double ProjTimeStep, ProjVelPeak;

  // desired trajectory (start and end point), not obeyed by all filters

  double TrajStart, TrajEnd;

};


// ****************************
// **                        **
// ****************************
inline void BHSpeedProfiler::SetTrajectory (const double startPos, const double endPos)
{
  TrajStart = startPos;
  TrajEnd = endPos;
}

// ****************************
// **                        **
// ****************************
inline double BHSpeedProfiler::ComputeTargetError (const double pos)
{
  return (EndPos - pos);
}

// ****************************
// **                        **
// ****************************
inline double BHSpeedProfiler::StretchProfile (const double tMin)
{
#ifdef DEBUG_1
  std::cout << " BHSpeedProfiler::StretchProfile()" << std::endl;
#endif

  return tMin;
}

// ****************************
// **                        **
// ****************************
inline double BHSpeedProfiler::GetTarget (void) const
{
  return EndPos;
}

// ****************************
// **                        **
// ****************************
inline double BHSpeedProfiler::GetTime (void) const
{
  return ProjTimeStep;
}
  
// ****************************
// **                        **
// ****************************
inline double BHSpeedProfiler::GetNewPosition (void) const
{
  return NextPos;
}

// ****************************
// **                        **
// ****************************
inline double BHSpeedProfiler::GetNewVelocity (void) const
{
  return NextVel;
}

//
// it is silently assumed that you first call `SetPositionAndVelocity()' if
// needed and then `SetTarget()'. the check for matching vector sizes will only
// work this way round...
//

// ****************************
// **                        **
// ****************************
inline void BHSpeedProfiler::SetTarget (const double endPos)
{
#ifdef DEBUG_1
  std::cout << " ## BHSpeedProfiler::SetTarget() : " << endPos <<  std::endl;
#endif

  

  EndPos = endPos;
}

#endif // BHSPEEDPROFILER_H

---

Tim Baier bhandlib v0.25

Generated Wed Aug 16 14:57:09 2006 by 1.4.7