Difference between revisions of "2. Allegro Hand Joint Position Control"
(→Allegro Hand Controller Basis) |
|||
(22 intermediate revisions by one user 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> | ||
− | + | myAHController.cpp | |
− | + | myAHController.h | |
− | + | myAHControllerCmd.h | |
</pre> | </pre> | ||
'''Goals:''' | '''Goals:''' | ||
− | # Create | + | # Create sinusoidal fashion hand motion to run at the start of AHAS.<br><br> |
− | <br> | + | |
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
− | + | ||
==Hand and Joint Initialization== | ==Hand and Joint Initialization== | ||
− | === | + | ===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"> | ||
... | ... | ||
− | + | 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( | + | , _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 | + | void myAHController::init(int mode) |
{ | { | ||
− | // this property is read from the | + | // 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 223: | Line 87: | ||
... | ... | ||
− | void | + | void myAHController::_arrangeJointDevices() |
{ | { | ||
− | |||
// _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 | ||
− | |||
// 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); | ||
− | |||
} | } | ||
Line 253: | Line 114: | ||
... | ... | ||
− | + | myAHController::~myAHController() | |
{ | { | ||
if (_hand) // if there is already a hand, | if (_hand) // if there is already a hand, | ||
Line 262: | Line 123: | ||
</syntaxhighlight> | </syntaxhighlight> | ||
− | === | + | ===myAHController.h=== |
− | We must now add all of the member variables and functions accessed to the class defined in the header file '' | + | 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 | + | #ifndef __MY_AH_CONTROLLER_H__ |
− | #define | + | #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 | + | #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> | ||
− | |||
− | |||
==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. | ||
− | === | + | ===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 | + | 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 | + | 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() | ||
− | |||
− | |||
<syntaxhighlight lang="cpp"> | <syntaxhighlight lang="cpp"> | ||
... | ... | ||
− | void | + | 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); | ||
− | |||
− | |||
} | } | ||
Line 376: | Line 249: | ||
</syntaxhighlight> | </syntaxhighlight> | ||
− | Now compile and run the code | + | Now compile and run the code. |
==Joint Position Control== | ==Joint Position Control== | ||
Line 385: | Line 258: | ||
... | ... | ||
− | void | + | void myAHController::_compute(const double& t) |
{ | { | ||
if (_hand) | if (_hand) | ||
Line 452: | 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. |
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:
- 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:
- _readDevices()
- _estimate()
- _reflect()
- _compute()
- _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 |