// *******************************************************************
// CPumaModel.h
//
// This file provides several implementations of IRobotModel for a
// puma.  These models work in conjunction with the drivers defined in
// CPuma.h and CSimPuma.h.
//
// modification history
// --------------------
//
// 07/07/04: Dan Merget: created
// *******************************************************************
#ifndef C_PUMA_MODEL_H
#define C_PUMA_MODEL_H

#include "IRobotModel.h"

// ============================================================================
// CPumaModelQ: The full 6-DOF Puma, using quaternions to represent the
// orientation.  The Jacobian is the basic jacobian.
// ============================================================================

class CPumaModelQ : public IRobotModel
{
public:
  CPumaModelQ();
  void analyzeState( IRobotSimDriver* evalRobot, Float sbound,
    PrVector& my_q, PrVector& my_dq,
    PrMatrix& my_J, PrMatrix& my_dJ,
    PrMatrix& my_A, PrVector& my_B, PrVector& my_G,
    int& my_singularities, PrVector& my_nullSpaceTau ) const;
  void getOperationalSpace( IRobotSimDriver* evalRobot, const PrVector& my_q,
    PrVector& my_x,
    PrMatrix& my_E, PrMatrix& my_Einverse ) const;
  void getFrame( const PrVector& x, PrTransform& frame ) const;
  void getConfigParms( const PrTransform& frame, PrVector& x ) const;
  void normalizeConfigParms( PrVector& x ) const;
};

// ============================================================================
// CPumaModelEA: The full 6-DOF Puma, using Euler angles to represent
// the orientation.  (This model uses the usual x-convention, in which
// we rotate around the z-axis, then y-axis, then z-axis.)  The
// Jacobian is the basic jacobian.
// ============================================================================

class CPumaModelEA : public CPumaModelQ
{
public:
  CPumaModelEA();
  void getOperationalSpace( IRobotSimDriver* evalRobot, const PrVector& my_q,
    PrVector& my_x,
    PrMatrix& my_E, PrMatrix& my_Einverse ) const;
  void getFrame( const PrVector& x, PrTransform& frame ) const;
  void getConfigParms( const PrTransform& frame, PrVector& x ) const;
  void normalizeConfigParms( PrVector& x ) const;
protected:
  void getEmatrix( Float psi, Float theta,
    PrMatrix& eMatrix, PrMatrix& eInverse ) const;
};

// ============================================================================
// CPumaModelEAY: The full 6-DOF Puma, representing the orientation
// with Euler Angles using the y-convention (i.e. rotate the z-y-z
// axes rather than the z-x-z axes).  This is a somewhat better match
// for the Puma end-effector.  The Jacobian is the basic jacobian.
// ============================================================================

class CPumaModelEAY : public CPumaModelEA
{
public:
  void getOperationalSpace( IRobotSimDriver* evalRobot, const PrVector& my_q,
    PrVector& my_x,
    PrMatrix& my_E, PrMatrix& my_Einverse ) const;
  void getFrame( const PrVector& x, PrTransform& frame ) const;
  void getConfigParms( const PrTransform& frame, PrVector& x ) const;
};

// ============================================================================
// CPumaModelAA: The full 6-DOF Puma, representing the orientation
// with axis-angles.  The Jacobian is the basic jacobian.
// ============================================================================

class CPumaModelAA : public CPumaModelQ
{
public:
  CPumaModelAA();
  void getOperationalSpace( IRobotSimDriver* evalRobot, const PrVector& my_q,
    PrVector& my_x,
    PrMatrix& my_E, PrMatrix& my_Einverse ) const;
  void getFrame( const PrVector& x, PrTransform& frame ) const;
  void getConfigParms( const PrTransform& frame, PrVector& x ) const;
  void normalizeConfigParms( PrVector& x ) const;
};

// ============================================================================
// CPuma1dofModel: A reduced 1-DOF puma in which only q2 moves.  Operational
// space is identical to joint space in this case.
// ============================================================================

class CPuma1dofModel : public IRobotModel
{
public:
  CPuma1dofModel();
  void analyzeState( IRobotSimDriver* evalRobot, Float sbound,
    PrVector& my_q, PrVector& my_dq,
    PrMatrix& my_J, PrMatrix& my_dJ,
    PrMatrix& my_A, PrVector& my_B, PrVector& my_G,
    int& my_singularities, PrVector& my_nullSpaceTau ) const;
  void getOperationalSpace( IRobotSimDriver* evalRobot, const PrVector& my_q,
    PrVector& my_x,
    PrMatrix& my_E, PrMatrix& my_Einverse ) const;
  void getFrame( const PrVector& x, PrTransform& frame ) const;
  void getConfigParms( const PrTransform& frame, PrVector& x ) const;
private:
  PrMatrix m_J;          // [1]
  PrMatrix m_dJ;         // [0]
  PrMatrix m_E;          // 1x1 identity matrix
  PrMatrix m_Einverse;   // 1x1 identity matrix
};

// ============================================================================
// CPuma3dofModel: A reduced 3-DOF puma in which we only use joints
// q2, q3, and q5 move.  The configuration parameters are x, z, and
// alpha, where alpha=q2+q3+q5 (alpha=0 points straight up).  The
// Jacobian uses the same convention, so the E matrix is a 3x3
// identity matrix.
// ============================================================================

class CPuma3dofModel : public IRobotModel
{
public:
  CPuma3dofModel();
  void analyzeState( IRobotSimDriver* evalRobot, Float sbound,
    PrVector& my_q, PrVector& my_dq,
    PrMatrix& my_J, PrMatrix& my_dJ,
    PrMatrix& my_A, PrVector& my_B, PrVector& my_G,
    int& my_singularities, PrVector& my_nullSpaceTau ) const;
  void getOperationalSpace( IRobotSimDriver* evalRobot, const PrVector& my_q,
    PrVector& my_x,
    PrMatrix& my_E, PrMatrix& my_Einverse ) const;
  void getFrame( const PrVector& x, PrTransform& frame ) const;
  void getConfigParms( const PrTransform& frame, PrVector& x ) const;
private:
  PrMatrix m_jacobianSelectionMatrix;   // 3x6 matrix used to convert basic
  // Jacobian to Jacobian
  PrMatrix m_E;               // 3x3 identity matrix
  PrMatrix m_Einverse;        // 3x3 identity matrix
};

// ============================================================================
// CPuma4dofModel: A reduced 3-DOF puma in which we only use joints
// q2, q3, and q5 move.  The configuration parameters are x, y, z, and
// alpha, where alpha=q2+q3+q5 (alpha=0 points straight up).  The
// Jacobian uses the same convention, so the E matrix is a 4x4
// identity matrix.
// ============================================================================

class CPuma4dofModel : public IRobotModel
{
public:
  CPuma4dofModel();
  void analyzeState( IRobotSimDriver* evalRobot, Float sbound,
    PrVector& my_q, PrVector& my_dq,
    PrMatrix& my_J, PrMatrix& my_dJ,
    PrMatrix& my_A, PrVector& my_B, PrVector& my_G,
    int& my_singularities, PrVector& my_nullSpaceTau ) const;
  void getOperationalSpace( IRobotSimDriver* evalRobot, const PrVector& my_q,
    PrVector& my_x,
    PrMatrix& my_E, PrMatrix& my_Einverse ) const;
  void getFrame( const PrVector& x, PrTransform& frame ) const;
  void getConfigParms( const PrTransform& frame, PrVector& x ) const;
private:
  PrMatrix m_jacobianSelectionMatrix;   // 4x6 matrix used to convert basic
  // Jacobian to Jacobian
  PrMatrix m_E;               // 4x4 identity matrix
  PrMatrix m_Einverse;        // 4x4 identity matrix
};

#endif // C_PUMA_MODEL_H
