(Joint Position Control)
 
(32 intermediate revisions by 2 users not shown)
Line 3: Line 3:
 
In the last tutorial, we created a Visual Studio (VS) project with three files:
 
In the last tutorial, we created a Visual Studio (VS) project with three files:
 
<pre>
 
<pre>
control_AllegroHand.cpp
+
myAHController.cpp
control_AllegroHand.h
+
myAHController.h
control_AllegroHandCmd.h
+
myAHControllerCmd.h
 
</pre>
 
</pre>
  
 
'''Goals:'''  
 
'''Goals:'''  
# Our first goal is to print the standard ''Hello World'' in the terminal window that accompanies AHAS. This will let us know that the controller is exporting to the correct directory, that the XDL is properly linking to the DLL and that the AHAS Lua file is, in fact, loading the correct controller for the virtual Allegro Hand.
+
# Create sinusoidal fashion hand motion to run at the start of AHAS.<br><br>
# Create a simple hand motion to run at the start of AHAS.
+
# Link this motion to a button in AHAS.
+
# Call some of the BHand library grasping motions using AHAS buttons.
+
<br>
+
 
+
The following code is the basis for your AHAS control plug-in. Please paste this code into the respective files, compile, then click the shortcut on the desktop to launch your custom AHAS application. In the command (terminal) window, you should see the confirming ''"Hello, World!"'', letting you know that you have everything set up correctly. You can now begin to develop your Allegro Hand controller.
+
 
+
For more information, please read the comments below and reference the ''RoboticsLab Programming Guide''.<br>
+
[[File:RoboticsLab_BegProgrammingGuide.pdf]] - Page 139, Section 4. ''Custom Control Algorithms''
+
 
+
===control_AllegroHand.h===
+
<syntaxhighlight lang="cpp">
+
 
+
#ifndef __CONTROL_ALLEGROHAND_H__
+
#define __CONTROL_ALLEGROHAND_H__
+
 
+
#include <list>
+
#include "rControlAlgorithm/rControlAlgorithm.h"
+
#include "BHand/BHand.h"
+
 
+
// 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:
+
+
// algorithm variables go here
+
};
+
 
+
#endif
+
 
+
</syntaxhighlight>
+
 
+
===control_AllegroHandCmd.h===
+
<syntaxhighlight lang="cpp">
+
 
+
#ifndef __CONTROL_ALLEGROHAND_CMD_H__
+
#define __CONTROL_ALLEGROHAND_CMD_H__
+
 
+
#include "rCommand/rCmdManipulator.h"
+
 
+
// These command values 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
+
 
+
</syntaxhighlight>
+
 
+
===control_AllegroHand.cpp===
+
<syntaxhighlight lang="cpp">
+
 
+
#include "control_AllegroHand.h"
+
#include "control_AllegroHandCmd.h"
+
 
+
 
+
control_AllegroHand::control_AllegroHand(rDC rdc)
+
:rControlAlgorithmEx(rdc)
+
{
+
// initialize all the class member variables
+
}
+
 
+
control_AllegroHand::~control_AllegroHand()
+
{
+
}
+
 
+
void control_AllegroHand::init(int mode)
+
{
+
// create hand and find devices
+
// arrange joint devices function will be called here
+
// set degrees of freedom
+
// make sure all vectors are the correct size and
+
// set all of the components to zero before computing
+
 
+
printf("\nHello World!");
+
}
+
 
+
void control_AllegroHand::_arrangeJointDevices()
+
{
+
// find and store all motors and encoders
+
}
+
 
+
void control_AllegroHand::update(const rTime& t)
+
{
+
// Evokes _readDevices(), _estimate(), _reflect(),
+
// _compute(), _writeDevices() in turn by default.
+
}
+
 
+
void control_AllegroHand::_readDevices()
+
{
+
// read sensors
+
}
+
 
+
void control_AllegroHand::_writeDevices()
+
{
+
// write to actuators
+
}
+
 
+
void control_AllegroHand::_compute(const double& t)
+
{
+
// Computes control inputs
+
}
+
 
+
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);
+
}
+
 
+
</syntaxhighlight>
+
  
 
==Hand and Joint Initialization==
 
==Hand and Joint Initialization==
  
===control_AllegroHand.cpp===
+
===myAHController.cpp===
  
 
In developing out controller for the Allegro Hand, lets start by initializing our hand and all of its devices. For now, we will focus on the function ''init()''. The code below will first initialize the hand, then call a secondary function, ''_arrangeJointDevices()'', to initialize all 16 of the motors and encoders for the hand. The initialize function will also make sure all arrays to store joint positions, velocities and torques are the length of the number of degrees of freedom (DOF) and set to zero (0) to start. The locations of the four (4) fingertips (x,y,z) are also set to zero when initialized.
 
In developing out controller for the Allegro Hand, lets start by initializing our hand and all of its devices. For now, we will focus on the function ''init()''. The code below will first initialize the hand, then call a secondary function, ''_arrangeJointDevices()'', to initialize all 16 of the motors and encoders for the hand. The initialize function will also make sure all arrays to store joint positions, velocities and torques are the length of the number of degrees of freedom (DOF) and set to zero (0) to start. The locations of the four (4) fingertips (x,y,z) are also set to zero when initialized.
 +
 +
Please, be careful when you copy and paste the following code.<br>
 +
There are skipped parts in the code.<br>
 +
You cannot copy and paste all at once.<br>
  
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
 
...
 
...
  
control_AllegroHand::control_AllegroHand(rDC rdc)  
+
myAHController::myAHController(rDC rdc)  
 
:rControlAlgorithmEx(rdc)
 
:rControlAlgorithmEx(rdc)
, _jdof(0)               // everything is NULL to start
+
, _jdof(0)                             // everything is NULL to start
 
, _hand(NULL)
 
, _hand(NULL)
 
, _is_left_hand(false)
 
, _is_left_hand(false)
, _demo_mode(0)   // will be used when
+
, _demo_mode(true)   // will be used when
, _demo_start_time(0)   // we make our own motion
+
, _demo_start_time(0)           // we make our own motion
 
{
 
{
 
}
 
}
Line 172: Line 36:
 
...
 
...
  
void control_AllegroHand::init(int mode)
+
void myAHController::init(int mode)
 
{
 
{
  
// this property is read from the control_AllegroHand XDL file
+
// this property is read from the myAHController XDL file
 
// to determine if the hand we are using will be the right or the left
 
// to determine if the hand we are using will be the right or the left
 
const TCHAR* prop = NULL;
 
const TCHAR* prop = NULL;
Line 182: Line 46:
 
if (prop && _tcsicmp(prop, _T("right")) == 0)
 
if (prop && _tcsicmp(prop, _T("right")) == 0)
 
{
 
{
_is_left_hand = false; // used to tell left from right
+
_is_left_hand = false;         // used to tell left from right
 
_hand = bhCreateRightHand(); // create the right hand
 
_hand = bhCreateRightHand(); // create the right hand
 
}
 
}
Line 192: Line 56:
 
}
 
}
 
assert(_hand); // if hand was not created, abort
 
assert(_hand); // if hand was not created, abort
_hand->SetTimeInterval(0.003); // control period is set (333Hz)
+
_hand->SetTimeInterval(0.003); // control period is set (333Hz)
 
 
 
_jdof = JDOF; // 16 DOF
 
_jdof = JDOF; // 16 DOF
  
_arrangeJointDevices(); // finds all hand motors and encoders
+
_arrangeJointDevices(); // finds all hand motors and encoders
 
 
_q.resize(_jdof); // array (16) holds current joint positions
+
_q.resize(_jdof);         // array (16) holds current joint positions
 
_qdot.resize(_jdof); // array (16) holds current joint velocities
 
_qdot.resize(_jdof); // array (16) holds current joint velocities
 
_torque.resize(_jdof); // array (16) holds current joint torques
 
_torque.resize(_jdof); // array (16) holds current joint torques
  
_q.zero(); // all positions, vel and torque set to zero
+
_q.zero();         // all positions, vel and torque set to zero
 
_qdot.zero();
 
_qdot.zero();
 
_torque.zero();
 
_torque.zero();
Line 209: Line 73:
 
_demo_q_des.zero(); // used to create motion sequences
 
_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(_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(_y, 0, sizeof(_y[0])*4); // for all four fingers
 
memset(_z, 0, sizeof(_z[0])*4);
 
memset(_z, 0, sizeof(_z[0])*4);
 
 
Line 223: Line 87:
 
...
 
...
  
void control_AllegroHand::_arrangeJointDevices()
+
void myAHController::_arrangeJointDevices()
 
{
 
{
printf("\nLooking for Motors and Encoders...\n\n");
 
 
// _jdof is 16.
 
// _jdof is 16.
 
for (int i=0; i<_jdof; i++)
 
for (int i=0; i<_jdof; i++)
Line 235: Line 98:
 
_stprintf(devname, _T("motor%d"), i + 1); // device name built and stored
 
_stprintf(devname, _T("motor%d"), i + 1); // device name built and stored
 
_motor[i] = findDevice(devname); // device located
 
_motor[i] = findDevice(devname); // device located
printf("found motor%d!\n", i + 1);
 
  
 
// find all 16 encoders on hand (enc1, enc2, ..., enc16)
 
// find all 16 encoders on hand (enc1, enc2, ..., enc16)
 
_stprintf(devname, _T("enc%d"), i + 1);
 
_stprintf(devname, _T("enc%d"), i + 1);
 
_enc[i] = findDevice(devname);
 
_enc[i] = findDevice(devname);
printf("found enc%d!\n\n", i + 1);
 
 
}
 
}
  
Line 253: Line 114:
 
...
 
...
  
control_AllegroHand::~control_AllegroHand()
+
myAHController::~myAHController()
 
{
 
{
 
if (_hand) // if there is already a hand,
 
if (_hand) // if there is already a hand,
Line 262: Line 123:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
===control_AllegroHand.h===
+
===myAHController.h===
  
We must now add all of the member variables and functions accessed to the class defined in the header file ''control_AllegroHand.h''.
+
We must now add all of the member variables and functions accessed to the class defined in the header file ''myAHController.h''.
  
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
#ifndef __CONTROL_ALLEGROHAND_H__
+
#ifndef __MY_AH_CONTROLLER_H__
#define __CONTROL_ALLEGROHAND_H__
+
#define __MY_AH_CONTROLLER_H__
 
+
 
#include <list>
 
#include <list>
 
#include "rControlAlgorithm/rControlAlgorithm.h"
 
#include "rControlAlgorithm/rControlAlgorithm.h"
 
#include "BHand/BHand.h"
 
#include "BHand/BHand.h"
  
#define JDOF 16 // degrees of freedom is 16
+
#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:
 
private:
+
virtual void _readDevices();
 +
virtual void _writeDevices();
 +
 +
virtual void _compute(const rTime& t);
 +
 +
void _arrangeJointDevices();
 +
 +
 +
private:
 +
 
// algorithm variables go here
 
// algorithm variables go here
 
rTime _cur_time;   // current time in controller
 
rTime _cur_time;   // current time in controller
 
+
 
BHand* _hand;   // Allegro Hand
 
BHand* _hand;   // Allegro Hand
 
bool _is_left_hand;   // bool, left?
 
bool _is_left_hand;   // bool, left?
 
+
 
rID _motor[16];   // motor array
 
rID _motor[16];   // motor array
 
rID _enc[16];   // encoder array
 
rID _enc[16];   // encoder array
 
+
 
dVector _q;   // joint current position
 
dVector _q;   // joint current position
 
dVector _qdot;   // joint current velocity
 
dVector _qdot;   // joint current velocity
 
dVector _torque;   // joint torque
 
dVector _torque;   // joint torque
 
+
 
int _jdof;   // degrees of freedom
 
int _jdof;   // degrees of freedom
 
+
 
double _x[4];   // location of each (4) fingertips
 
double _x[4];   // location of each (4) fingertips
 
double _y[4];   // in x, y, and z
 
double _y[4];   // in x, y, and z
 
double _z[4];
 
double _z[4];
 
+
 
int _demo_mode;   // bool, used later as flag to envoke user control
 
int _demo_mode;   // bool, used later as flag to envoke user control
 
rTime _demo_start_time;  // time at which the demo mode starts
 
rTime _demo_start_time;  // time at which the demo mode starts
 
dVector _demo_q_des;   // desired position for position controller
 
dVector _demo_q_des;   // desired position for position controller
 
+
 
};
 
};
  
...
+
#endif
 
</syntaxhighlight>
 
</syntaxhighlight>
 
At this point, if you compile and run AHAS by clicking your desktop shortcut, you should see command window confirmation that all 16 motors and 16 encoders have been found.
 
  
 
==Updating the Controller==
 
==Updating the Controller==
Line 314: Line 191:
 
Now that we have initialized all the joint devices, we can employ a simple joint controller. This joint controller employs PID control and is part of the BHand library.
 
Now that we have initialized all the joint devices, we can employ a simple joint controller. This joint controller employs PID control and is part of the BHand library.
  
===control_AllegroHand.cpp===
+
===myAHController.cpp===
 
Before actually writing the joint commands, we must first set up the controller to read the joint positions for the encoder devices and write joint torque to the motor devices.
 
Before actually writing the joint commands, we must first set up the controller to read the joint positions for the encoder devices and write joint torque to the motor devices.
  
Line 320: Line 197:
 
...
 
...
  
void control_AllegroHand::_readDevices()
+
void myAHController::_readDevices()
 
{
 
{
 
// all 16 encoder pos. values are read and stored in _q[]
 
// all 16 encoder pos. values are read and stored in _q[]
Line 334: Line 211:
 
}
 
}
  
void control_AllegroHand::_writeDevices()
+
void myAHController::_writeDevices()
 
{
 
{
 
// all 16 motor _torque[] values are written to the motor device
 
// all 16 motor _torque[] values are written to the motor device
Line 359: Line 236:
 
#_compute()
 
#_compute()
 
#_writeDevices()
 
#_writeDevices()
 
As seen in the function, we will add a statement to print the current time to the command window. This will confirm that our simulation is updating every control iteration. Once it is confirmed to work, comment or delete this print statement to avoid flooding the command window with numbers.
 
  
 
<syntaxhighlight lang="cpp">
 
<syntaxhighlight lang="cpp">
 
...
 
...
  
void control_AllegroHand::update(const rTime& t)
+
void myAHController::update(const rTime& t)
 
{
 
{
 
_cur_time = t; // controller is updated every control period
 
_cur_time = t; // controller is updated every control period
 
rControlAlgorithm::update(t);
 
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.
 
 
}
 
}
  
Line 376: Line 249:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Now compile and run the code. You should see a stream of numbers (current time) running down the command window. This confirms that the controller is updating properly. Go back and comment out this print statement then continue on with the tutorial.
+
Now compile and run the code.
  
 
==Joint Position Control==
 
==Joint Position Control==
Line 385: Line 258:
 
...
 
...
  
void control_AllegroHand::_compute(const double& t)
+
void myAHController::_compute(const double& t)
 
{
 
{
 
if (_hand)
 
if (_hand)
 
{
 
{
 +
_hand->SetJointPosition(_q.array); // joint positions send to Bhand
 +
 
if (true)
 
if (true)
 
{
 
{
 
_hand->SetMotionType(eMotionType_JOINT_PD);    // PID gains and motion control set
 
_hand->SetMotionType(eMotionType_JOINT_PD);    // PID gains and motion control set
_hand->SetJointPosition(_q.array); // joint positions set via torque PID
 
 
 
 
static float sin_speed = 0.3;
 
static float sin_speed = 0.3;
Line 423: Line 297:
 
{
 
{
 
_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ;
 
_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ;
 
 
}
 
}
 
}
 
}
Line 433: Line 306:
 
{
 
{
 
_demo_q_des[i*NOJ+j] = q_home_right[i][j] + delQ;
 
_demo_q_des[i*NOJ+j] = q_home_right[i][j] + delQ;
printf("%fR\n ",_demo_q_des[i*NOF+j]);
 
 
 
}
 
}
 
}
 
}
Line 441: Line 312:
 
_hand->SetJointDesiredPosition(_demo_q_des.array);
 
_hand->SetJointDesiredPosition(_demo_q_des.array);
 
 
 
 
}
 
}
  
// controller, current finger tip position, and torque are updated
+
// controller is updated in BHand
 +
// current finger tip position, and torque are updated from BHand
 
_hand->UpdateControl(t);
 
_hand->UpdateControl(t);
 
_hand->GetFKResult(_x, _y, _z);
 
_hand->GetFKResult(_x, _y, _z);
Line 454: Line 325:
 
</syntaxhighlight>
 
</syntaxhighlight>
  
Compile once again and run AHAS from the shortcut on your ''Desktop''. You should now see every joint moving in a sinusoidal fashion.
+
Compile once again and run AHAS from the shortcut on your ''Desktop''. You should now see every joint moving in a sinusoidal fashion.<br>
 +
You can see the result in this [[File:allegro_tutorial_2_result.zip]].<br>
 +
If you can't see the same result as above, download this [[File:AllegroHandAppStudio_bhand_patch.zip]], unzip and paste bin, lib and include folders into<br> C:\Program Files (x86)\SimLab\Allegro Hand Application Studio folder.
  
 
In the next tutorial, we will demonstrate how easy it is to try out this controller on your actual Allegro Hand.
 
In the next tutorial, we will demonstrate how easy it is to try out this controller on your actual Allegro Hand.
  
 
[[3. Interfacing with the Actual Allegro Hand]]
 
[[3. Interfacing with the Actual Allegro Hand]]
 +
[[Category:Programming Guides]]

Latest revision as of 15:51, 30 April 2015

Contents

[edit] Allegro Hand Controller Basis

In the last tutorial, we created a Visual Studio (VS) project with three files:

myAHController.cpp
myAHController.h
myAHControllerCmd.h

Goals:

  1. Create sinusoidal fashion hand motion to run at the start of AHAS.

[edit] Hand and Joint Initialization

[edit] myAHController.cpp

In developing out controller for the Allegro Hand, lets start by initializing our hand and all of its devices. For now, we will focus on the function init(). The code below will first initialize the hand, then call a secondary function, _arrangeJointDevices(), to initialize all 16 of the motors and encoders for the hand. The initialize function will also make sure all arrays to store joint positions, velocities and torques are the length of the number of degrees of freedom (DOF) and set to zero (0) to start. The locations of the four (4) fingertips (x,y,z) are also set to zero when initialized.

Please, be careful when you copy and paste the following code.
There are skipped parts in the code.
You cannot copy and paste all at once.

...
 
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
{
}
 
...
 
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);
 
}
 
...

As the function _arrangeJointDevices() is called during initialization, we must also fill in this function to find all of the motor and encoder devices on the hand. The names used to find these devices can be found in the Allegro Hand AML model file. The motors are stored and accessed via the _motor[] array and the encoders are stored and accessed via the _enc[] array.

...
 
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);
	}
 
}
 
...

Lastly, just in case a hand already exist when we try to make a controller, lets add a piece of code that will delete any hand that already exists before creating a new one.

...
 
myAHController::~myAHController()
{
	if (_hand)		// if there is already a hand,
		delete _hand;	// delete it before creating a new one
}
 
...

[edit] myAHController.h

We must now add all of the member variables and functions accessed to the class defined in the header file 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

[edit] Updating the Controller

Now that we have initialized all the joint devices, we can employ a simple joint controller. This joint controller employs PID control and is part of the BHand library.

[edit] myAHController.cpp

Before actually writing the joint commands, we must first set up the controller to read the joint positions for the encoder devices and write joint torque to the motor devices.

...
 
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);
		}
	}
}
 
...

To cause these updates to happen periodically, we must update the controller each iteration to step through time.

Each time the controller is updated, it automatically calls the following functions in order:

  1. _readDevices()
  2. _estimate()
  3. _reflect()
  4. _compute()
  5. _writeDevices()
...
 
void myAHController::update(const rTime& t)
{
	_cur_time = t;			// controller is updated every control period
	rControlAlgorithm::update(t);
}
 
...

Now compile and run the code.

[edit] Joint Position Control

We will now employ a simple algorithm to change the joint position with time in a sinusoidal fashion. This will be done using the BHand function SetJointDesiredPosition(double* q).

...
 
void myAHController::_compute(const double& t)
{
	if (_hand)
	{
		_hand->SetJointPosition(_q.array);			// joint positions send to Bhand
 
		if (true)
		{
			_hand->SetMotionType(eMotionType_JOINT_PD);     // PID gains and motion control set
 
			static float sin_speed = 0.3;
			static float sin_amp = 10;
 
			// this is the home position for the right hand
			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}
			};
 
			// this is the home position for the left hand
			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}
			};
 
 
			_demo_q_des.zero();
 
			if (_is_left_hand)
			{
				double delQ = sin_amp*DEGREE*sin(2.0*M_PI*sin_speed*(_cur_time-_demo_start_time));
				for (int i=0; i<NOF; i++)		// for fingers 1 to 4
					for (int j=0; j<NOJ; j++)	// for joints 1 to 4
					{
						_demo_q_des[i*NOF+j] = q_home_left[i][j] + delQ;
					}
			}
			else
			{
				double delQ = sin_amp*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*NOJ+j] = q_home_right[i][j] + delQ;
					}
			}
 
			// joint position is set using torque PID control
			_hand->SetJointDesiredPosition(_demo_q_des.array);
 
		}
 
		// controller is updated in BHand
		// current finger tip position, and torque are updated from BHand
		_hand->UpdateControl(t);
		_hand->GetFKResult(_x, _y, _z);
		_hand->GetJointTorque(_torque.array);
	}
}
 
...

Compile once again and run AHAS from the shortcut on your Desktop. You should now see every joint moving in a sinusoidal fashion.
You can see the result in this File:Allegro tutorial 2 result.zip.
If you can't see the same result as above, download this File:AllegroHandAppStudio bhand patch.zip, unzip and paste bin, lib and include folders into
C:\Program Files (x86)\SimLab\Allegro Hand Application Studio folder.

In the next tutorial, we will demonstrate how easy it is to try out this controller on your actual Allegro Hand.

3. Interfacing with the Actual Allegro Hand





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