/***************************************************************
 * PrTreeRobotReal.h
 *
 * This class provides the TreeRobotReal robot interface.
 *
 ****************************************************************/

/*
 * modification history
 *----------------------
 *
 * 11/17/98: K.C. Chang: modified to real robot only.
 * 09/02/98: K.C. Chang: created.
 */

#ifndef _PrTreeRobotReal_h
#define _PrTreeRobotReal_h

#define PR_TREEROBOTREAL_ERROR 0
#define PR_TREEROBOTREAL_OK    1

#define PR_TREEROBOTREAL_POWER_OFF  0
#define PR_TREEROBOTREAL_POWER_ON   1

#define PR_TREEROBOTREAL_NONE    0
#define PR_TREEROBOTREAL_SAMM0   1
#define PR_TREEROBOTREAL_SAMM1   2

#include "PrGlobalDefn.h"
#include "driver/PrIPBoardIntf.h"
#include "matrix/PrVector6.h"

#include "PrTreeRobotIntf.h"

#include "IPBoardStg.h" 

class PrRobot;
class PrVector;

class PrTreeRobotReal : public PrTreeRobotIntf
{

public:
	PrTreeRobotReal(int noj,char *paramFilename,int servoRate,int robotType=PR_TREEROBOTREAL_NONE);
	virtual ~PrTreeRobotReal();
	virtual int noj() const;
	virtual int mode() const { return mode_; } 
	virtual void mode(int m) { mode_ = m; }
	virtual int modeGripper() const { return emode_; } 
	virtual void modeGripper(int m) { emode_ = m; }
	virtual void jointVel(const PrVector &dq);
	virtual PrVector &jointPos();
	virtual PrVector &jointVel();
	virtual PrVector &computedTorque(const PrVector &tau_prime);
	virtual void torque(const PrVector &tau);
                void torque2(const PrVector &tau, const PrVector &tauExt);

	virtual Float gripperPos();
	virtual Float gripperVel();
	virtual void gripperTorque(Float t);
	virtual PrVector6 &fsValue();
	virtual void fsZero();

	virtual PrRobot *robot() { return robot_; }

	int flag() const { return flag_; }
	void power(int p = PR_TREEROBOTREAL_POWER_ON);
	void updatePositionVelocity();

        IPBoardStg *hwInterface() { return hwInterface_; }
        
  private:
	const int servoRate_;
	int mode_;
	int emode_;
	int flag_;
	PrRobot *robot_;
	IPBoardStg *hwInterface_;
	
	PrVector *q_;
	PrVector *dq_;
	PrVector *tau_;

	PrVector6 f_;
	Float fs_[6];

	// copy is not allowed
	PrTreeRobotReal(const PrTreeRobotReal &) : servoRate_(0) {}
};

#endif // _PrTreeRobotReal_h


