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

Latest revision as of 18:44, 30 April 2015

In the past 4 tutorials, we learned how to control the Allegro Hand using the included grasping and motions library. In this tutorials, we will take a step back and control the hand at a lower level. We will go over basic filtering of the encoder data and PD position control of the joints.

For this lesson, we must roll back our controller code to the point before we implemented any code to move the hand joints. It may be a better idea to start a new project with the following code. Remember, you configure the project according to the DLL project properties from tutorial 1. Your code should reflect the code developed in the three steps of tutorial 2:

myAHController.cpp

#include "myAHController.h"
#include "myAHControllerCmd.h"
 
 
myAHController::myAHController(rDC rdc) 
:rControlAlgorithmEx(rdc)
, _jdof(0)                              // everything is NULL to start
, _hand(NULL)
, _is_left_hand(false)
, _demo_mode(true)		  // will be used when
, _demo_start_time(0)	          // we make our own motion
{
}
 
myAHController::~myAHController()
{
	if (_hand)		// if there is already a hand,
		delete _hand;	// delete it before creating a new one
}
 
void myAHController::init(int mode)
{
 
	// this property is read from the myAHController 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
	}
	// if using a left hand
	else
	{
		_is_left_hand = true;
		_hand = bhCreateLeftHand();	// create the left hand
	}
	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 myAHController::_arrangeJointDevices()
{
	// _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
 
		// find all 16 encoders on hand (enc1, enc2, ..., enc16)
		_stprintf(devname, _T("enc%d"), i + 1);
		_enc[i] = findDevice(devname);
	}
 
}
 
void myAHController::update(const rTime& t)
{
	_cur_time = t;			// controller is updated every control period
	rControlAlgorithm::update(t);
}
 
void myAHController::_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 myAHController::_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 myAHController::_compute(const double& t)
{
	// Computes control inputs
}
 
int myAHController::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 myAHController(rdc);
}

myAHController.h

#ifndef __MY_AH_CONTROLLER_H__
#define __MY_AH_CONTROLLER_H__
 
#include <list>
#include "rControlAlgorithm/rControlAlgorithm.h"
#include "BHand/BHand.h"
 
#define JDOF	16
 
// myAHController inherited from algorithm interface class
class REXPORT myAHController : public rControlAlgorithmEx
{
public:
	myAHController(rDC rdc);
	~myAHController();
 
	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:
 
	// algorithm variables go here
	rTime		_cur_time;	   // current time in controller
 
	BHand*		_hand;		   // Allegro Hand
	bool		_is_left_hand;	   // bool, left?
 
	rID		_motor[16];	   // motor array
	rID		_enc[16];	   // encoder array
 
	dVector		_q;		   // joint current position
	dVector		_qdot;		   // joint current velocity
	dVector		_torque;	   // joint torque
 
	int		_jdof;		   // degrees of freedom
 
	double		_x[4];		   // location of each (4) fingertips
	double		_y[4];		   // in x, y, and z
	double		_z[4];
 
	int		_demo_mode;	   // bool, used later as flag to envoke user control
	rTime		_demo_start_time;  // time at which the demo mode starts
	dVector		_demo_q_des;	   // desired position for position controller
 
};
 
#endif

myAHControllerCmd.h

#ifndef __MY_AH_CONTROLLER_CMD_H__
#define __MY_AH_CONTROLLER_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

[edit] Check the code

Let's quickly compile the code above to make sure we copied everything correctly. It should compile with no errors.

Next, open up AHAS via your virtual Allegro Hand shortcut. None of the buttons should work anymore but you should see the command prompt indication that all motors and encoders have been found. These are all we need!

[edit] Reading and Writing

We have two functions, _readDevices() and _writeDevices(), that are called every control period. For the Allegro Hand controller, this means that the encoder values are accessed and motor commands are calculated and written at 333Hz, or once every 0.003 seconds.

When _readDevices() accessed the encoder data, it saves it to the array _q[]" which has 16 floating point entries (once for each joint). The joint positions in _q are in radians.

Similarly, when writing a torque command to the motors, "_writeDevices()" accesses the 16-long floating-point array _torque[]'. The torque values are in the unit Newton-meters.

Alike _readDevices() and "_writeDevices()", the function '_compute() is also called every control iteration. This is where we will implement are controller code, or the computation of motor torque based on joint positions.

Remember: The 16 joint torque commands and joint positions are stored in float arrays with 16 entries each and indexed as follows:

Finger	Joint	Index
1	1	0
1	2	1
1	3	2
1	4	3
2	1	4
2	2	5
2	3	6
2	4	7
3	1	8
3	2	9
3	3	10
3	4	11
4	1	12
4	2	13
4	3	14
4	4	15

Let's add a bit of code to apply a small torque to all four (4) joints on the index finger and subsequently read the joint position of each and print it to the command window.

[edit] myAHController.h

#ifndef __MY_AH_CONTROLLER_H__
#define __MY_AH_CONTROLLER_H__
 
#include <list>
#include "rControlAlgorithm/rControlAlgorithm.h"
#include "BHand/BHand.h"
 
#define JDOF	16
 
// myAHController inherited from algorithm interface class
class REXPORT myAHController : public rControlAlgorithmEx
{
public:
	myAHController(rDC rdc);
	~myAHController();
 
	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:
 
	// algorithm variables go here
	rTime		_cur_time;	   // current time in controller
 
	BHand*		_hand;		   // Allegro Hand
	bool		_is_left_hand;	   // bool, left?
 
	rID		_motor[16];	   // motor array
	rID		_enc[16];	   // encoder array
 
	dVector		_q;		   // joint current position
	dVector		_qdot;		   // joint current velocity
	dVector		_torque;	   // joint torque
 
	int		_jdof;		   // degrees of freedom
 
	double		_x[4];		   // location of each (4) fingertips
	double		_y[4];		   // in x, y, and z
	double		_z[4];
 
	int		_demo_mode;	   // bool, used later as flag to envoke user control
	rTime		_demo_start_time;  // time at which the demo mode starts
	dVector		_demo_q_des;	   // desired position for position controller
	FILE* _fp; 
};
 
#endif

[edit] myAHController.cpp

myAHController::myAHController(rDC rdc) 
:rControlAlgorithmEx(rdc)
, _jdof(0)                              // everything is NULL to start
, _hand(NULL)
, _is_left_hand(false)
, _demo_mode(true)		  // will be used when
, _demo_start_time(0)	          // we make our own motion
, _fp(NULL)
{
	fopen_s(&fp, "C:\\myAHController.txt", "w");
}
 
myAHController::~myAHController()
{
	if (_hand)		// if there is already a hand,
		delete _hand;	// delete it before creating a new one
 
	if (_fp)
		fclose(_fp);
}
 
...
 
void myAHController::_compute(const double& t)
{
	// Computes control inputs
        // Joints 0, 1, 2 and 3 are commanded at 0.1 N.m
	for (int i=0; i<4; i++)
	{
		_torque[i] = 0.1;
	}
 
        // Joint positions 0, 1, 2 and 3 are printed
	for (int i=0; i<4; i++)
	{
		fprintf(_fp, "%i: %f\t",i,_q[i]);
	}
	fprintf(_fp, "\n"); // next line
}

Compile and execute the shortcut in the Desktop.
It is necessary to execute the shortcut with Administrator Privilege.
You can see the file that has joint positions in C:\myAHController.txt file.
We can now design any controller with a position input and a torque output.

[edit] PD Control

One of the simplest and most widely used control algorithms, Proportional-Derivative (PD) control can be easily implemented to control the position of a robotic joint.

To use PD control, we need to calculate the error between the joint's set, or desired, position and the joint's current position. For each joint, this error value and a gain value will comprise the proportional controller. Along with the current error, we will also calculate the error's rate of change from time-step to time-step. This will be calculated using the difference between the current and previous error values divided by the time-step.

Whos here now: Members 0 Guests 0 Bots & Crawlers 1

@@ Line 1: / Line 1: @@
 In the past 4 tutorials, we learned how to control the Allegro Hand using the included grasping and motions library. In this tutorials, we will take a step back and control the hand at a lower level. We will go over basic filtering of the encoder data and PD position control of the joints.
-For this lesson, we must roll back our controller code to the point before we implemented any code to move the hand joints. It may be a better idea to start a new project with the following code. Remember, you configure the project according to the DLL project properties from tutorial 1.
+For this lesson, we must roll back our controller code to the point before we implemented any code to move the hand joints. It may be a better idea to start a new project with the following code. Remember, you configure the project according to the DLL project properties from tutorial 1. Your code should reflect the code developed in the three steps of tutorial 2:
 *[[2._Allegro_Hand_Joint_Position_Control#Allegro_Hand_Controller_Basis | 2.1 Allegro Hand Controller Basis]]
@@ Line 7: / Line 7: @@
 *[[2._Allegro_Hand_Joint_Position_Control#Updating_the_Controller | 2.3 Updating the Controller]]
-<br><br>
 <div class="toccolours mw-collapsible mw-collapsed">
+'''myAHController.cpp'''
-====control_ERHand.cpp====
 <div class="mw-collapsible-content">
 <syntaxhighlight lang="cpp">
-/* RoboticsLab, Copyright 2008-2010 SimLab Co., Ltd. All rights reserved.
-*
-* This library is commercial and cannot be redistributed, and/or modified
-* WITHOUT ANY ALLOWANCE OR PERMISSION OF SimLab Co., LTD.
-*/
-#include "control_ERHand.h"
-#include "control_ERHandCmd.h"
-//#define DEBUG_COMM
+#include "myAHController.h"
+#include "myAHControllerCmd.h"
-control_ERHand::control_ERHand(rDC rdc)
+myAHController::myAHController(rDC rdc)
 :rControlAlgorithmEx(rdc)
-, _cur_time(0)
+, _jdof(0)                              // everything is NULL to start
-, _jdof(0)
-, _jid_test(0)
-, _test_mode(false)
 , _hand(NULL)
 , _is_left_hand(false)
-, _demo_mode(0)
+, _demo_mode(true)		  // will be used when
-, _demo_start_time(0)
+, _demo_start_time(0)	          // we make our own motion
-, _RPS(0)
-, _zero(0)
 {
 }
-control_ERHand::~control_ERHand()
+myAHController::~myAHController()
 {
-	if (_hand)
+	if (_hand)		// if there is already a hand,
-		delete _hand;
+		delete _hand;	// delete it before creating a new one
-}
-void control_ERHand::_servoOn()
-{
-}
-void control_ERHand::_servoOff()
-{
-}
-void control_ERHand::_arrangeJointDevices()
-{
-	for (int i=0; i<_jdof; i++)
-	{
-		TCHAR devname[32];
-		_stprintf(devname, _T("motor%d"), i + 1);
-		_motor[i] = findDevice(devname);
-		_stprintf(devname, _T("enc%d"), i + 1);
-		_enc[i] = findDevice(devname);
-	}
 }
-void control_ERHand::init(int mode)
+void myAHController::init(int mode)
 {
+	// this property is read from the myAHController 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;
+		_is_left_hand = false;		        // used to tell left from right
-		_hand = bhCreateRightHand();
+		_hand = bhCreateRightHand();	// create the right hand
 	}
+	// if using a left hand
 	else
 	{
 		_is_left_hand = true;
-		_hand = bhCreateLeftHand();
+		_hand = bhCreateLeftHand();	// create the left hand
 	}
-	assert(_hand);
+	assert(_hand);				// if hand was not created, abort
-	_hand->SetTimeInterval(0.003);
+	_hand->SetTimeInterval(0.003);	// control period is set (333Hz)
-	_jdof = JDOF;
+	_jdof = JDOF;				// 16 DOF
-	_arrangeJointDevices();
+	_arrangeJointDevices();		// finds all hand motors and encoders
-	_remoteTP_BT	= findDevice(_T("RemoteTP_BT"));
+	_q.resize(_jdof);			        // array (16) holds current joint positions
-	_remoteTP_TCPIP	= findDevice(_T("RemoteTP_TCPIP"));
+	_qdot.resize(_jdof);			// array (16) holds current joint velocities
+	_torque.resize(_jdof);			// array (16) holds current joint torques
-	_q.resize(_jdof);
-	_qdot.resize(_jdof);
+	_q.zero();				        // all positions, vel and torque set to zero
-	_torque.resize(_jdof);
-	_q.zero();
 	_qdot.zero();
 	_torque.zero();
-	memset(_x, 0, sizeof(_x[0])*4);
+	_demo_q_des.resize(_jdof);		// desired joint positions
-	memset(_y, 0, sizeof(_y[0])*4);
+	_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);
-	_sz2read_msgRTP = sizeof(MessageRemoteTP_t);
-	_demo_q_des.resize(_jdof);
-	_demo_q_des.zero();
 }
+void myAHController::_arrangeJointDevices()
-void control_ERHand::update(const rTime& t)
 {
-	_cur_time = t;
+	// _jdof is 16.
-	rControlAlgorithm::update(t);
+	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
+		// find all 16 encoders on hand (enc1, enc2, ..., enc16)
+		_stprintf(devname, _T("enc%d"), i + 1);
+		_enc[i] = findDevice(devname);
+	}
 }
-void control_ERHand::setNominalSystem(const TCHAR* path, const TCHAR* aml, const HTransform& T0, const dVector& q0)
+void myAHController::update(const rTime& t)
 {
+	_cur_time = t;			// controller is updated every control period
+	rControlAlgorithm::update(t);
 }
-void control_ERHand::setPeriod(const rTime& dT)
+void myAHController::_readDevices()
-{
-}
-void control_ERHand::_readDevices()
 {
+	// all 16 encoder pos. values are read and stored in _q[]
 	float val;
 	for (int i=0; i<JDOF; i++)
@@ Line 137: / Line 113: @@
 		}
 	}
-	// read message from bluetooth
-	if(_remoteTP_BT != INVALID_RHANDLE)
-	{
-		int szRead = readDeviceValue(_remoteTP_BT, &_msgRTP, _sz2read_msgRTP, DEVPORT_FETCH_MESSAGE);
-		if(szRead == _sz2read_msgRTP && szRead > 0)
-		{
-#ifdef DEBUG_COMM
-			printf("type : %d, size : %d, cmd : %d, data : %d\n", _msgRTP.type, _msgRTP.size, _msgRTP.cmd, _msgRTP.data);
-#endif
-			if(_msgRTP.type == RTP_TYPE_COMMAND_EVENT)
-			{
-				switch(_msgRTP.cmd)
-				{
-				case RTP_CMD_CMD1:
-					{
-						command(BH_HOME);
-					}
-					break;
-				case RTP_CMD_CMD2:
-					{
-						command(BH_READY);
-					}
-					break;
-				case RTP_CMD_CMD3:
-					{
-						command(BH_GRASP_3);
-					}
-					break;
-				case RTP_CMD_CMD4:
-					{
-						command(BH_GRASP_4);
-					}
-					break;
-				case RTP_CMD_CMD5:
-					{
-						command(BH_PINCH_IT);
-					}
-					break;
-				case RTP_CMD_CMD6:
-					{
-						command(BH_PINCH_MT);
-					}
-					break;
-				case RTP_CMD_CMD7:
-					{
-						command(BH_MOVE_OBJ);
-					}
-					break;
-				case RTP_CMD_CMD8:
-					{
-						command(BH_ENVELOP);
-					}
-					break;
-				case RTP_CMD_CMD9:
-					{
-						printf("reserved command[9]\n");
-					}
-					break;
-				case RTP_CMD_CMD10:
-					{
-						printf("reserved command[10]\n");
-					}
-					break;
-				case RTP_CMD_CMD11:
-					{
-						command(BH_SHOWOFF);
-					}
-					break;
-				case RTP_CMD_CMD12:
-					{
-						printf("reserved command[12]\n");
-					}
-					break;
-				}
-			}
-		}
-	} // bluetooth
-	// read message from TCP/IP
-	if(_remoteTP_TCPIP != INVALID_RHANDLE)
-	{
-		int szRead = readDeviceValue(_remoteTP_TCPIP, &_msgRTP, _sz2read_msgRTP, DEVPORT_FETCH_MESSAGE);
-		if(szRead == _sz2read_msgRTP && szRead > 0)
-		{
-#ifdef DEBUG_COMM
-			printf("type : %d, size : %d, cmd : %d, data : %d\n", _msgRTP.type, _msgRTP.size, _msgRTP.cmd, _msgRTP.data);
-#endif
-			if(_msgRTP.type == RTP_TYPE_COMMAND_EVENT)
-			{
-				switch(_msgRTP.cmd)
-				{
-				case RTP_CMD_CMD1:
-					{
-						command(BH_HOME);
-					}
-					break;
-				case RTP_CMD_CMD2:
-					{
-						command(BH_READY);
-					}
-					break;
-				case RTP_CMD_CMD3:
-					{
-						command(BH_GRASP_3);
-					}
-					break;
-				case RTP_CMD_CMD4:
-					{
-						command(BH_GRASP_4);
-					}
-					break;
-				case RTP_CMD_CMD5:
-					{
-						command(BH_PINCH_IT);
-					}
-					break;
-				case RTP_CMD_CMD6:
-					{
-						command(BH_PINCH_MT);
-					}
-					break;
-				case RTP_CMD_CMD7:
-					{
-						command(BH_MOVE_OBJ);
-					}
-					break;
-				case RTP_CMD_CMD8:
-					{
-						command(BH_ENVELOP);
-					}
-					break;
-				case RTP_CMD_CMD9:
-					{
-						printf("reserved command[9]\n");
-					}
-					break;
-				case RTP_CMD_CMD10:
-					{
-						printf("reserved command[10]\n");
-					}
-					break;
-				case RTP_CMD_CMD11:
-					{
-						command(BH_SHOWOFF);
-					}
-					break;
-				case RTP_CMD_CMD12:
-					{
-						printf("reserved command[12]\n");
-					}
-					break;
-				}
-			}
-		}
-	} // TCP/IP
 }
-void control_ERHand::_writeDevices()
+void myAHController::_writeDevices()
 {
+	// all 16 motor _torque[] values are written to the motor device
 	float val;
 	for (int i=0; i<JDOF; i++)
@@ Line 329: / Line 128: @@
 	}
 }
-void control_ERHand::_reflect()
+void myAHController::_compute(const double& t)
 {
+	// Computes control inputs
 }
-void control_ERHand::_compute(const double& t)
+int myAHController::command(const short& cmd, const int& arg)
 {
-	if (_hand)
+	// Handles user-defined commands according to cmd.
-	{
+	// Further information can be retrieved from the second argument.
-		if (_test_mode)
-		{
+	// The variable cmd will be received from Allegro Application Studio
-			// TEST1: constant torque
+	// and will be used to envoke hand actions
-			_torque.zero();
-			_torque[_jid_test] = 200.0f / 800.0f;
-			// TEST2: PD control
-			/*double tau_tmp = _torque[_jid_test];
-			if (tau_tmp < -0.5) tau_tmp = -0.5;
-			else if (tau_tmp > 0.5) tau_tmp = 0.5;
-			_torque.zero();
-			_torque[_jid_test] = tau_tmp;*/
-		}
-		else
-		{
-			_hand->SetJointPosition(_q.array);
-			if (_demo_mode == 1)
-			{
-				static float period = 20; //total loop time
-				static float second_move_time = 10;
-				static float third_move_time = 20;
-				static float sin_speed = 0.3;
-				static double q_home_left[NOF][NOJ] = {
-					{  0*DEG2RAD,	-10*DEG2RAD,	45*DEG2RAD,		45*DEG2RAD},
-					{  0*DEG2RAD,	-10*DEG2RAD,	45*DEG2RAD,		45*DEG2RAD},
-					{ -5*DEG2RAD,	 -5*DEG2RAD,	50*DEG2RAD,		45*DEG2RAD},
-					{ 50*DEG2RAD,	 25*DEG2RAD,	15*DEG2RAD,		45*DEG2RAD}
-				};
-				static double q_home_right[NOF][NOJ] = {
-					{  0*DEG2RAD,	-10*DEG2RAD,	45*DEG2RAD,		45*DEG2RAD},
-					{  0*DEG2RAD,	-10*DEG2RAD,	45*DEG2RAD,		45*DEG2RAD},
-					{  5*DEG2RAD,	 -5*DEG2RAD,	50*DEG2RAD,		45*DEG2RAD},
-					{ 50*DEG2RAD,	 25*DEG2RAD,	15*DEG2RAD,		45*DEG2RAD}
-				};
-				float elapsed_time = _cur_time-_demo_start_time;
-				//printf("elapsed_time = %f\n", elapsed_time);
-				_demo_q_des.zero();
-				if (fmod(elapsed_time,period) < second_move_time)
-				{
-					//printf("1");
-					if (_is_left_hand)
-					{
-						double delQ = 30*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time))*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time));
-						double delQ_about0 = 30*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time));
-						for (int i=0; i<NOF; i++)
-							for (int j=0; j<NOJ; j++)
-							{
-								_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ;
-								if ((i<3)&&(j==0))
-									_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ_about0;
-								//_demo_q_des[i*NOF+j] = q_home_left[i][j];
-							}
-					}
-					else
-					{
-						double delQ = 30*DEGREE*sin(2.0*M_PI*0.3*_cur_time-_demo_start_time)*sin(2.0*M_PI*0.3*_cur_time-_demo_start_time);
-						for (int i=0; i<NOF; i++)
-							for (int j=0; j<NOJ; j++)
-								_demo_q_des[i*NOJ+j] = q_home_right[i][j] + delQ;
-					}
-					_hand->SetJointDesiredPosition(_demo_q_des.array);
-				}
-				if ((fmod(elapsed_time,period) > second_move_time)&&(fmod(elapsed_time,period) < third_move_time))
-				{
-					//printf("2");
-					if (_is_left_hand)
-					{
-						double delQ_0 = 30*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time)) ; // for first joint on three fingers
-						double delQ = 30*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time))*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time)) ;
-						double delQ_f1 = 30*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time)-M_PI/4)*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time)-M_PI/4) - 0.2;
-						double delQ_f2 = 30*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time))*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time)) - 0.2;
-						double delQ_f3 = 30*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time)+M_PI/4)*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time)+M_PI/4) - 0.2;
-						//printf("\n\n%f\n\n",delQ_f1);
-						int i=0; // for finger 1
-						for (int j=1; j<NOJ; j++) // for joints 2, 3 and 4
-							_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ_f1;
-						i=1; // for finger 2
-						for (int j=1; j<NOJ; j++) // for joints 2, 3 and 4
-							_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ_f2;
-						i=2; // for finger 3
-						for (int j=1; j<NOJ; j++) // for joints 2, 3 and 4
-							_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ_f3;
-						i=3; // for thumb
-						for (int j=1; j<NOJ; j++) // for joints 2, 3 and 4
-							_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ_f1;
-						int j=0;
-						for (int i=0; i<(NOF-1); i++) // for joint 1 on fingers 1 2 3
-							_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ_0;
-						i = 3;
-						j = 0;
-						_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ;
-					}
-					else
-					{
-						double delQ = 40*DEGREE*sin(2.0*M_PI*0.5*_cur_time-_demo_start_time)*sin(2.0*M_PI*0.5*_cur_time-_demo_start_time);
-						for (int i=0; i<NOF; i++)
-							for (int j=0; j<NOJ; j++)
-								_demo_q_des[i*NOJ+j] = q_home_right[i][j] + delQ;
-					}
-					_hand->SetJointDesiredPosition(_demo_q_des.array);
-				}
-			} // end demo_mode 1
-			// ROCK PAPER SCISSORS
-			if (_demo_mode == 2)
-			{
-				_demo_q_des.zero();
-				double elapsed_time =  _cur_time - _demo_start_time;
-				static double rock[] = {0.1194, 1.2068, 1.2479, 1.4042, 0.0093, 1.2481, 1.4073, 0.8163, -0.1116, 1.2712, 1.3881, 1.0122, 0.6017, 0.2976, 0.9034, 0.7929};
-				static double paper[] = {0.1220, -0.1267, 0.4416, -0.0769, -0.0312, 0.0078, 0.2045, -0.0, -0.1767, 0.0342, 0.2609, -0.0528, 0.5284, 0.3693, 0.8977, 0.4863};
-				static double scissors[] = {-0.0885, -0.0189, 0.2718, -0.0704, -0.0312, 0.0078, 0.2045, -0.0, -0.1019, 1.2375, 1.1346, 1.0244, 1.1457, 0.6331, 1.2509, 0.6132};
-				if (elapsed_time<0.1)
-				{
-					//printf("1");
-					for (int i=0; i<NOF*NOJ; i++)
-					{
-						_demo_q_des[i] = _q[i] + 0.2;
-						if (i == 12)
-							_demo_q_des[i] = _q[i] - 0.4;
-					}
-				}
-				else {
-					switch (_RPS)
-					{
-					case 1:
-						{
-							//printf("1");
-							for (int i=0; i<NOF*NOJ; i++)
-								_demo_q_des[i] = rock[i];
-						}
-						break;
-					case 2:
-						{
-							//printf("1");
-							for (int i=0; i<NOF*NOJ; i++)
-								_demo_q_des[i] = paper[i];
-						}
-						break;
-					case 3:
-						{
-							//printf("1");
-							for (int i=0; i<NOF*NOJ; i++)
-								_demo_q_des[i] = scissors[i];
-						}
-						break;
-					default:
-						break;
-					}
-				}
-				_hand->SetJointDesiredPosition(_demo_q_des.array);
-			}
-			if (_zero == 1)
-			{
-				_demo_q_des.zero();
-				_demo_q_des[12] = 0.90; // thumb
-				_hand->SetJointDesiredPosition(_demo_q_des.array);
-			}
-			/////////// update after any motion
-			_hand->UpdateControl(t);
-			_hand->GetFKResult(_x, _y, _z);
-			_hand->GetJointTorque(_torque.array);
-		}
-	}
-}
-void control_ERHand::_estimate()
-{
-}
-int control_ERHand::command(const short& cmd, const int& arg)
-{
-	//if (cmd == BH_TEST)
-	//{
-	//	_test_mode = true;
-	//	_demo_mode = 0;
-	//	_jid_test++;
-	//	if (_jid_test >= JDOF)
-	//		_jid_test = 0;
-	//}
-	//else if (cmd == BH_SHOWOFF)
-	//{
-	//	_test_mode = false;
-	//	_demo_mode = 1;
-	//	_demo_start_time = _cur_time;
-	//	if (_hand)
-	//		_hand->SetMotionType(eMotionType_JOINT_PD);
-	//}
-	//else
-	//{
-	_test_mode = false;
-	_demo_mode = 0;
-	_zero = 0;
-	switch (cmd)
-	{
-	case BH_NONE:
-		{
-			if (_hand)
-			{
-				_hand->SetMotionType(eMotionType_NONE);
-				// Read and Print encoder vales
-				printf("\nCurrent Encoder Readings:");
-				_hand->SetJointPosition(_q.array);
-				for (int i=0; i<(NOF*NOJ); i++)
-				{
-					printf("\nENC %i:\t%f", (i+1), _q[i]);
-				}
-				printf("\n");
-			}
-		}
-		break;
-	case BH_HOME:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_HOME);
-		}
-		break;
-	case BH_TEST:
-		{
-			_test_mode = true;
-			//_demo_mode = 0;
-			//_zero = 0;
-			_jid_test++;
-			if (_jid_test >= JDOF)
-				_jid_test = 0;
-		}
-		break;
-	case BH_SHOWOFF:
-		{
-			//_test_mode = false;
-			_demo_mode = 1;
-			_demo_start_time = _cur_time;
-			//_zero = 0;
-			if (_hand)
-				_hand->SetMotionType(eMotionType_JOINT_PD);
-		}
-		break;
-	case BH_RPS: //rock paper scissors
-		{
-			//_test_mode = false;
-			_demo_mode = 2;
-			_demo_start_time = _cur_time;
-			//_zero = 0;
-			_RPS = rand() % 3 +1;
-			printf("%i",_RPS);
-			if (_hand)
-				_hand->SetMotionType(eMotionType_JOINT_PD);
-			printf("\nROCK PAPER SCISSORS\n");
-		}
-		break;
-	case BH_ALPHAMOD: //rock paper scissors
-		{
-			_hand->GetGraspingForce(_fx, _fy, _fz);
-			_f_norm[0] = sqrt(_fx[0]*_fx[0] + _fy[0]*_fy[0] + _fz[0]*_fz[0]);
-			_f_norm[1] = sqrt(_fx[1]*_fx[1] + _fy[1]*_fy[1] + _fz[1]*_fz[1]);
-			_f_norm[2] = sqrt(_fx[2]*_fx[2] + _fy[2]*_fy[2] + _fz[2]*_fz[2]);
-			_f_norm[3] = sqrt(_fx[3]*_fx[3] + _fy[3]*_fy[3] + _fz[3]*_fz[3]);
-			printf("Index:\t%f\n", _f_norm[0]);
-			printf("Middle:\t%f\n",_f_norm[1]);
-			printf("Pinky:\t%f\n", _f_norm[2]);
-			printf("Thumb:\t%f\n\n", _f_norm[3]);
-			double addForce = 0.3;
-			if(arg == VK_UP)
-			{
-				_f_norm[0] += addForce;
-				_f_norm[1] += addForce;
-				_f_norm[2] += 0;
-				_f_norm[3] += addForce;
-			}
-			else
-			{
-				_f_norm[0] -= addForce;
-				_f_norm[1] -= addForce;
-				_f_norm[2] -= 0;
-				_f_norm[3] -= addForce;
-			}
-			_hand->SetGraspingForce(_f_norm);
-		}
-		break;
-	case BH_ZERO: //rock paper scissors
-		{
-			//_test_mode = false;
-			//_demo_mode = 0;
-			_zero = 1;
-			if (_hand)
-				_hand->SetMotionType(eMotionType_JOINT_PD);
-			printf("\nZERO POS\n");
-		}
-		break;
-	case BH_READY:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_READY);
-		}
-		break;
-	case BH_GRASP_3:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_GRASP_3);
-		}
-		break;
-	case BH_GRASP_4:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_GRASP_4);
-		}
-		break;
-	case BH_PINCH_IT:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_PINCH_IT);
-		}
-		break;
-	case BH_PINCH_MT:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_PINCH_MT);
-		}
-		break;
-	case BH_MOVE_OBJ:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_OBJECT_MOVING);
-		}
-		break;
-	case BH_ENVELOP:
-		{
-			if (_hand)
-				_hand->SetMotionType(eMotionType_ENVELOP);
-		}
-		break;
-	case RESERVED_CMD_SERVO_ON:
-		_servoOn();
-		break;
-	case RESERVED_CMD_SERVO_OFF:
-		_servoOff();
-		break;
-	default:
-		break;
-	}
-	//}
 	return 0;
 }
-void control_ERHand::datanames(vector<string_type>& names, int channel)
-{
-	switch (channel)
-	{
-	case ePlotManipulator::PLOT_TORQUE:
-		{
-			TCHAR dname[64];
-			for (int i=0; i<NOF; i++) {
-				for (int j=0; j<NOJ; j++) {
-					_stprintf(dname, _T("tau_des[%d][%d]"), i, j);
-					names.push_back(dname);
-				}
-			}
-		}
-		break;
-	}
-}
-void control_ERHand::collect(vector<double>& data, int channel)
-{
-	switch (channel)
-	{
-	case ePlotManipulator::PLOT_TORQUE:
-		{
-			for (int i=0; i<NOF*NOJ; i++)
-				data.push_back(_torque[i]);
-		}
-		break;
-	case 255:
-		{
-			data.push_back(_x[0]);
-			data.push_back(_x[1]);
-			data.push_back(_x[2]);
-			data.push_back(_x[3]);
-		}
-		break;
-	case 254:
-		{
-			data.push_back(_y[0]);
-			data.push_back(_y[1]);
-			data.push_back(_y[2]);
-			data.push_back(_y[3]);
-		}
-		break;
-	case 253:
-		{
-			data.push_back(_z[0]);
-			data.push_back(_z[1]);
-			data.push_back(_z[2]);
-			data.push_back(_z[3]);
-		}
-		break;
-	}
-}
-void control_ERHand::onSetInterestFrame(const TCHAR* name, const HTransform& T)
-{
-}
 rControlAlgorithm* CreateControlAlgorithm(rDC& rdc)
 {
-	return new control_ERHand(rdc);
+	 return new myAHController(rdc);
 }
@@ Line 830: / Line 153: @@
 </div>
 <br>
 <div class="toccolours mw-collapsible mw-collapsed">
+'''myAHController.h'''
-====control_ERHand.h====
 <div class="mw-collapsible-content">
 <syntaxhighlight lang="cpp">
-/* RoboticsLab, Copyright 2008-2010 SimLab Co., Ltd. All rights reserved.
-*
-* This library is commercial and cannot be redistributed, and/or modified
-* WITHOUT ANY ALLOWANCE OR PERMISSION OF SimLab Co., LTD.
-*/
-#ifndef __CONTROL_ERHAND_H__
-#define __CONTROL_ERHAND_H__
+#ifndef __MY_AH_CONTROLLER_H__
+#define __MY_AH_CONTROLLER_H__
 #include <list>
 #include "rControlAlgorithm/rControlAlgorithm.h"
 #include "BHand/BHand.h"
-#include "rDeviceERHandRemoteCmd.h"
+#define JDOF	16
-#define JDOF 16
+// myAHController inherited from algorithm interface class
-class REXPORT control_ERHand : public rControlAlgorithmEx
+class REXPORT myAHController : public rControlAlgorithmEx
 {
 public:
-	control_ERHand(rDC rdc);
+	myAHController(rDC rdc);
-	~control_ERHand();
+	~myAHController();
 	virtual void init(int mode = 0);
 	virtual void update(const rTime& t);
-	virtual void setNominalSystem(const TCHAR* path, const TCHAR* aml, const HTransform& T0, const dVector& q0);
+	virtual int  command(const short& cmd, const int& arg = 0);
-	virtual void setPeriod(const rTime& dT);
-	virtual int command(const short& cmd, const int& arg = 0);
-	virtual void datanames(vector<string_type>& names, int channel = -1);
-	virtual void collect(vector<double>& data, int channel = -1);
-	virtual void onSetInterestFrame(const TCHAR* name, const HTransform& T);
 private:
-	virtual void _estimate();
 	virtual void _readDevices();
 	virtual void _writeDevices();
-	virtual void _reflect();
 	virtual void _compute(const rTime& t);
 	void _arrangeJointDevices();
+private:
+	// algorithm variables go here
+	rTime		_cur_time;	   // current time in controller
+	BHand*		_hand;		   // Allegro Hand
+	bool		_is_left_hand;	   // bool, left?
+	rID		_motor[16];	   // motor array
+	rID		_enc[16];	   // encoder array
+	dVector		_q;		   // joint current position
+	dVector		_qdot;		   // joint current velocity
+	dVector		_torque;	   // joint torque
+	int		_jdof;		   // degrees of freedom
+	double		_x[4];		   // location of each (4) fingertips
+	double		_y[4];		   // in x, y, and z
+	double		_z[4];
+	int		_demo_mode;	   // bool, used later as flag to envoke user control
+	rTime		_demo_start_time;  // time at which the demo mode starts
+	dVector		_demo_q_des;	   // desired position for position controller
+};
+#endif
-	void _servoOn();
+</syntaxhighlight>
-	void _servoOff();
+</div>
+</div>
+<br>
+<div class="toccolours mw-collapsible mw-collapsed">
-private:
+'''myAHControllerCmd.h'''
-	rTime				_cur_time;
+<div class="mw-collapsible-content">
+<syntaxhighlight lang="cpp">
-	BHand*				_hand;
+#ifndef __MY_AH_CONTROLLER_CMD_H__
-	bool				_is_left_hand;
+#define __MY_AH_CONTROLLER_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
-	rID					_motor[16];
+</syntaxhighlight>
-	rID					_enc[16];
+</div>
-	rID					_remoteTP_BT;
+</div>
-	rID					_remoteTP_TCPIP;
+<br>
-	dVector				_q;
+==Check the code==
-	dVector				_qdot;
+Let's quickly compile the code above to make sure we copied everything correctly. It should compile with no errors.
-	dVector				_torque;
-	int					_jdof;
+Next, open up AHAS via your virtual Allegro Hand shortcut. None of the buttons should work anymore but you should see the command prompt indication that all motors and encoders have been found. These are all we need!
-	double				_x[4];
+==Reading and Writing==
-	double				_y[4];
+We have two functions, ''_readDevices()'' and ''_writeDevices()'', that are called every control period. For the Allegro Hand controller, this means that the encoder values are accessed and motor commands are calculated and written at 333Hz, or once every 0.003 seconds.
-	double				_z[4];
-	double				_fx[4];
-	double				_fy[4];
-	double				_fz[4];
-	double				_f_norm[4];
+When ''_readDevices()'' accessed the encoder data, it saves it to the array ''_q[]" which has 16 floating point entries (once for each joint). The joint positions in ''_q'' are in radians.
-	// for testing each motor
+Similarly, when writing a torque command to the motors, "_writeDevices()" accesses the 16-long floating-point array ''_torque[]'. The torque values are in the unit Newton-meters.
-	int					_jid_test;
-	bool				_test_mode;
-	// for Rock Paper Scissors
-	int					_RPS;
-	int					_zero;
-	// for remote control
+Alike ''_readDevices()'' and "_writeDevices()", the function '_compute()'' is also called every control iteration. This is where we will implement are controller code, or the computation of motor torque based on joint positions.
-	MessageRemoteTP_t	_msgRTP;
-	int					_sz2read_msgRTP;
-	// for show off
-	int					_demo_mode;
+'''Remember:''' The 16 joint torque commands and joint positions are stored in float arrays with 16 entries each and indexed as follows:
-	rTime				_demo_start_time;
-	dVector				_demo_q_des;
+{|style=" border-collapse: separate; border-spacing: 15; border-width: 2px; border-style: solid; border-color: #CCCCCC; padding:  0px; text-align: center; width: 200px"
+|-style="background: #D0D0D0"
+|'''Finger'''
+|'''Joint'''
+|'''Index'''
+|-style="background: #F0F0F0"
+|1||1||0
+|-
+|1||2||1
+|-
+|1||3||2
+|-
+|1||4||3
+|-style="background: #F0F0F0"
+|2||1||4
+|-
+|2||2||5
+|-
+|2||3||6
+|-
+|2||4||7
+|-style="background: #F0F0F0"
+|3||1||8
+|-
+|3||2||9
+|-
+|3||3||10
+|-
+|3||4||11
+|-style="background: #F0F0F0"
+|4||1||12
+|-
+|4||2||13
+|-
+|4||3||14
+|-
+|4||4||15
+|}
+Let's add a bit of code to apply a small torque to all four (4) joints on the index finger and subsequently read the joint position of each and print it to the command window.
+===myAHController.h===
+<syntaxhighlight lang="cpp">
+#ifndef __MY_AH_CONTROLLER_H__
+#define __MY_AH_CONTROLLER_H__
+#include <list>
+#include "rControlAlgorithm/rControlAlgorithm.h"
+#include "BHand/BHand.h"
+#define JDOF	16
+// myAHController inherited from algorithm interface class
+class REXPORT myAHController : public rControlAlgorithmEx
+{
+public:
+	myAHController(rDC rdc);
+	~myAHController();
+	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:
+	// algorithm variables go here
+	rTime		_cur_time;	   // current time in controller
+	BHand*		_hand;		   // Allegro Hand
+	bool		_is_left_hand;	   // bool, left?
+	rID		_motor[16];	   // motor array
+	rID		_enc[16];	   // encoder array
+	dVector		_q;		   // joint current position
+	dVector		_qdot;		   // joint current velocity
+	dVector		_torque;	   // joint torque
+	int		_jdof;		   // degrees of freedom
+	double		_x[4];		   // location of each (4) fingertips
+	double		_y[4];		   // in x, y, and z
+	double		_z[4];
+	int		_demo_mode;	   // bool, used later as flag to envoke user control
+	rTime		_demo_start_time;  // time at which the demo mode starts
+	dVector		_demo_q_des;	   // desired position for position controller
+	FILE* _fp;
 };
 #endif
 </syntaxhighlight>
-</div>
-</div>
 <br>
-<div class="toccolours mw-collapsible mw-collapsed">
+===myAHController.cpp===
-====control_ERHandCMD.h====
-<div class="mw-collapsible-content">
 <syntaxhighlight lang="cpp">
-/* RoboticsLab, Copyright 2008-2010 SimLab Co., Ltd. All rights reserved.
-*
-* This library is commercial and cannot be redistributed, and/or modified
-* WITHOUT ANY ALLOWANCE OR PERMISSION OF SimLab Co., LTD.
-*/
-#ifndef __CONTROL_ERHAND_CMD_H__
-#define __CONTROL_ERHAND_CMD_H__
-#include "rCommand/rCmdManipulator.h"
+myAHController::myAHController(rDC rdc)
+:rControlAlgorithmEx(rdc)
+, _jdof(0)                              // everything is NULL to start
+, _hand(NULL)
+, _is_left_hand(false)
+, _demo_mode(true)		  // will be used when
+, _demo_start_time(0)	          // we make our own motion
+, _fp(NULL)
+{
+	fopen_s(&fp, "C:\\myAHController.txt", "w");
+}
+myAHController::~myAHController()
+{
+	if (_hand)		// if there is already a hand,
+		delete _hand;	// delete it before creating a new one
-#define BH_NONE		(RCMD_USER + 0)
+	if (_fp)
-#define BH_HOME		(RCMD_GO_HOME)
+		fclose(_fp);
-#define BH_READY	(RCMD_USER + 2)
+}
-#define BH_GRASP_3	(RCMD_USER + 3)
-#define BH_GRASP_4	(RCMD_USER + 4)
-#define BH_PINCH_IT	(RCMD_USER + 5)
-#define BH_PINCH_MT	(RCMD_USER + 6)
-#define BH_MOVE_OBJ	(RCMD_USER + 7)
-#define BH_ENVELOP	(RCMD_USER + 8)
+...
-#define BH_SHOWOFF	(RCMD_USER + 50)
+void myAHController::_compute(const double& t)
-#define BH_RPS		(RCMD_USER + 51)
+{
+	// Computes control inputs
+        // Joints 0, 1, 2 and 3 are commanded at 0.1 N.m
+	for (int i=0; i<4; i++)
+	{
+		_torque[i] = 0.1;
+	}
+        // Joint positions 0, 1, 2 and 3 are printed
+	for (int i=0; i<4; i++)
+	{
+		fprintf(_fp, "%i: %f\t",i,_q[i]);
+	}
+	fprintf(_fp, "\n"); // next line
+}
+</syntaxhighlight>
-#define BH_ALPHAMOD	(RCMD_USER + 90)
+Compile and execute the shortcut in the Desktop.<br>
+It is necessary to execute the shortcut with Administrator Privilege.<br>
+You can see the file that has joint positions in C:\myAHController.txt file.<br>
+We can now design any controller with a position input and a torque output.
+==PD Control==
-#define BH_TEST		(RCMD_USER + 99)
+One of the simplest and most widely used control algorithms, Proportional-Derivative (PD) control can be easily implemented to control the position of a robotic joint.
-#define BH_ZERO		(RCMD_USER + 100)
-#endif
+To use PD control, we need to calculate the error between the joint's set, or desired, position and the joint's current position. For each joint, this error value and a gain value will comprise the proportional controller. Along with the current error, we will also calculate the error's rate of change from time-step to time-step. This will be calculated using the difference between the current and previous error values divided by the time-step.
+[[Category:Programming Guides]]
-</syntaxhighlight>
-</div>
-</div>
-<br>