robFunctionSE3.h

Go to the documentation of this file.

/*
  Author(s): Simon Leonard
  Created on: Nov 11 2009

  (C) Copyright 2008-2014 Johns Hopkins University (JHU), All Rights Reserved.

--- begin cisst license - do not edit ---

This software is provided "as is" under an open source license, with
no warranty.  The complete license can be found in license.txt and
http://www.cisst.org/cisst/license.txt.

--- end cisst license ---
*/

#ifndef _robFunctionSE3_h
#define _robFunctionSE3_h

#include <cisstVector/vctFixedSizeVector.h>
#include <cisstVector/vctFrame4x4.h>

#include <cisstRobot/robFunction.h>
#include <cisstRobot/robFunctionRn.h>
#include <cisstRobot/robFunctionSO3.h>
#include <cisstRobot/robExport.h>


class CISST_EXPORT robFunctionSE3 : public robFunction {

protected:

  vctFrame4x4<double> Rtw1;              // start tranform
  vctFixedSizeVector<double,6> v1w1;     // start velocity
  vctFixedSizeVector<double,6> v1dw1d;   // start acceleration

  vctFrame4x4<double> Rtw2;              // stop  transform
  vctFixedSizeVector<double,6> v2w2;     // stop  velocity
  vctFixedSizeVector<double,6> v2dw2d;   // stop  acceleration

  robFunctionRn* translation;     // robFunciton for Rn (R3 for SE3 case)
  robFunctionSO3* rotation;       // robFunction for SO3 (Rotation)

public:

  robFunctionSE3( void );

  robFunctionSE3( double t1,
          const vctFrame4x4<double>& Rtw1,
          const vctFixedSizeVector<double,6>& v1w1,
          const vctFixedSizeVector<double,6>& v1dw1d,
          double t2,
          const vctFrame4x4<double>& Rtw2,
          const vctFixedSizeVector<double,6>& v2w2,
          const vctFixedSizeVector<double,6>& v2dw2d );

  virtual ~robFunctionSE3();

  void InitialState( vctFrame4x4<double>& Rtw1,
             vctFixedSizeVector<double,6>& v1w1,
             vctFixedSizeVector<double,6>& v1dw1d );


  void FinalState( vctFrame4x4<double>& Rtw2,
           vctFixedSizeVector<double,6>& v2w2,
           vctFixedSizeVector<double,6>& v2dw2d );

  virtual void Evaluate( double,
             vctFrame4x4<double>&,
             vctFixedSizeVector<double,6>&,
             vctFixedSizeVector<double,6>&) = 0;

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

  robFunctionSE3& operator=( const robFunctionSE3& function );

};

#endif // _robFunctionSE3_h