robQLQRn.h

Go to the documentation of this file.

#ifndef _robQLQRn_h
#define _robQLQRn_h

#include <cisstRobot/robLinearRn.h>
#include <cisstRobot/robQuintic.h>
#include <cisstRobot/robExport.h>

class CISST_EXPORT robQLQRn : public robFunctionRn {

 private:

  vctDynamicVector<double> qdmax;
  vctDynamicVector<double> qddmax;
  
  //double toffset;
  double tauin;
  double tauout;

  robQuintic*  transitionin;
  robLinearRn* cruise;
  robQuintic*  transitionout;

  double EvaluateTransitionTime( const vctDynamicVector<double>& q1d,
                 const vctDynamicVector<double>& q2d );
    
  void CreateInputTransition( double tau,
                  const vctDynamicVector<double>& q1, 
                  const vctDynamicVector<double>& q1d, 
                  const vctDynamicVector<double>& q1dd );

  void CreateOutputTransition( double tau,
                   const vctDynamicVector<double>& q2, 
                   const vctDynamicVector<double>& q2d, 
                   const vctDynamicVector<double>& q2dd );


 public:

  robQLQRn( const vctDynamicVector<double>& q1,
        const vctDynamicVector<double>& q2,
        const vctDynamicVector<double>& qd,
        const vctDynamicVector<double>& qdd,
        double t1 = 0.0,
        bool forceblend = false );

  void Evaluate( double t, 
         vctDynamicVector<double>& q,
         vctDynamicVector<double>& qd,
         vctDynamicVector<double>& qdd );

  void Blend( robFunction* function,
          const vctDynamicVector<double>& qdmax, 
          const vctDynamicVector<double>& qddmax );

  void Blend( robFunction* function, double vmax, double vdmax );

};

#endif