Difference between revisions of "5. Joint PD Control Without Hand Library"

#include "control_AllegroHand.h"
#include "control_AllegroHandCmd.h"
 
#define JDOF 16
 
control_AllegroHand::control_AllegroHand(rDC rdc) 
:rControlAlgorithmEx(rdc)
, _jdof(0) // everything is NULL to start
, _hand(NULL)
, _is_left_hand(false)
, _demo_mode(0)			// will be used when
, _demo_start_time(0)	// we make our own motion
{
}
 
control_AllegroHand::~control_AllegroHand()
{
	if (_hand)			// if there is already a hand,
		delete _hand;	// delete it before creating a new one
}
 
 
void control_AllegroHand::init(int mode)
{
 
	// this property is read from the control_AllegroHand XDL file
	// to determine if the hand we are using will be the right or the left
	const TCHAR* prop = NULL;
	prop = getProperty(_T("whichHand"));
	// if using a right hand
	if (prop && _tcsicmp(prop, _T("Right")) == 0)
	{
		_is_left_hand = false;		// used to tell left from right
		_hand = bhCreateRightHand();	// create the right hand
		printf("RIGHT Hand Created.");
	}
	// if using a left hand
	else
	{
		_is_left_hand = true;
		_hand = bhCreateLeftHand();	// create the left hand
		printf("LEFT Hand Created.");
	}
	assert(_hand);				// if hand was not created, abort
	_hand->SetTimeInterval(0.003);		// control period is set (333Hz)
 
	_jdof = JDOF;				// 16 DOF
 
	_arrangeJointDevices();			// finds all hand motors and encoders
 
	_q.resize(_jdof);			// array (16) holds current joint positions
	_qdot.resize(_jdof);			// array (16) holds current joint velocities	
	_torque.resize(_jdof);			// array (16) holds current joint torques
 
	_q.zero();				// all positions, vel and torque set to zero
	_qdot.zero();
	_torque.zero();
 
	_demo_q_des.resize(_jdof);		// desired joint positions
	_demo_q_des.zero();			// used to create motion sequences
 
	memset(_x, 0, sizeof(_x[0])*4);		// sets x, y and z position to (0,0,0)
	memset(_y, 0, sizeof(_y[0])*4);		// for all four fingers
	memset(_z, 0, sizeof(_z[0])*4);
 
}
 
void control_AllegroHand::_arrangeJointDevices()
{
	printf("\nLooking for Motors and Encoders...\n\n");
	// _jdof is 16.
	for (int i=0; i<_jdof; i++)
	{
		// oversited array intialized for storing the device name string
		TCHAR devname[32];
 
		// find all 16 motors on hand (motor1, motor2, ..., motor16)
		_stprintf(devname, _T("motor%d"), i + 1);	// device name built and stored
		_motor[i] = findDevice(devname);			// device located
		printf("found motor%d!\n", i + 1);
 
		// find all 16 encoders on hand (enc1, enc2, ..., enc16)
		_stprintf(devname, _T("enc%d"), i + 1);
		_enc[i] = findDevice(devname);
		printf("found enc%d!\n\n", i + 1);
	}
 
}
 
void control_AllegroHand::update(const rTime& t)
{
	_cur_time = t;			// controller is updated every control period
	rControlAlgorithm::update(t);
	//printf("\n%f", _cur_time ); 	// for testing whether or not the controller is updating
									// if it works, make sure to comment this out before running again.
}
 
void control_AllegroHand::_readDevices()
{
	// all 16 encoder pos. values are read and stored in _q[]
	float val;
	for (int i=0; i<JDOF; i++)
	{
		if (_enc[i] != INVALID_RID)
		{
			readDeviceValue(_enc[i], &val, 4);
			_q[i] = (float)val;
		}
	}
}
 
void control_AllegroHand::_writeDevices()
{
	// all 16 motor _torque[] values are written to the motor device
	float val;
	for (int i=0; i<JDOF; i++)
	{
		val = (float)_torque[i];
		if (_motor[i] != INVALID_RID)
		{
			writeDeviceValue(_motor[i], &val, 4);
		}
	}
}
 
void control_AllegroHand::_compute(const double& t)
{
	// calculate control input pased on sensory data here
}
 
int control_AllegroHand::command(const short& cmd, const int& arg)
{
	// Handles user-defined commands according to cmd.
	// Further information can be retrieved from the second argument.
 
	// The variable cmd will be received from Allegro Application Studio
	// and will be used to envoke hand actions
	return 0;
}
 
rControlAlgorithm* CreateControlAlgorithm(rDC& rdc)
{
	 return new control_AllegroHand(rdc);
}

#ifndef __CONTROL_ALLEGROHAND_H__
#define __CONTROL_ALLEGROHAND_H__
 
#include <list>
#include "rControlAlgorithm/rControlAlgorithm.h"
#include "BHand/BHand.h"
 
#define JDOF 16
 
// control_AllegroHand inherited from algorithm interface class
class REXPORT control_AllegroHand : public rControlAlgorithmEx
{
public:
	control_AllegroHand(rDC rdc);
	~control_AllegroHand();
 
	virtual void init(int mode = 0);
	virtual void update(const rTime& t);
	virtual int  command(const short& cmd, const int& arg = 0);
 
private:
	virtual void _readDevices();
	virtual void _writeDevices();
 
	virtual void _compute(const rTime& t);
 
	void _arrangeJointDevices();
 
 
private:
	rTime				_cur_time;
 
	BHand*				_hand;
	bool				_is_left_hand;
 
	rID					_motor[16];
	rID					_enc[16];
 
	dVector				_q;
	dVector				_qdot;
	dVector				_torque;
 
	int					_jdof;
 
	double				_x[4];
	double				_y[4];
	double				_z[4];
 
	int					_demo_mode;
	rTime				_demo_start_time;
	dVector				_demo_q_des;
 
};
 
#endif

#ifndef __CONTROL_ALLEGROHAND_CMD_H__
#define __CONTROL_ALLEGROHAND_CMD_H__
 
#include "rCommand/rCmdManipulator.h"
 
// These commands will be fed into command()
// and can be used to envoke certain actions
// by the robot. Allegro Application Studio
// will use these to interface with the
// cotroller plug-in.
#define BH_NONE		(RCMD_USER + 0)
#define BH_HOME		(RCMD_GO_HOME)
// #define BH_ONE		(RCMD_USER + 1)
 
#endif