5. Joint PD Control Without Hand Library
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 "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); }
myAHController.h
#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]; }; #endif
myAHControllerCmd.h
#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
Contents |
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!
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.
control_AllegroHand.cpp
void control_AllegroHand::_compute(const double& t) { // 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++) { printf("%i: %f\t",i,_q[i]); } printf("\n"); // next line }
That was easy! We can now design any controller with a position input and a torque output.
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.
Copyright & Trademark Notice
Allegro, the Allegro logo, RoboticsLab, the RoboticsLab logo, and all related files and documentation are Copyright ⓒ 2008-2020 Wonik Robotics Co., Ltd. All rights reserved. RoboticsLab and Allegro are trademarks of Wonik Robotics. All other trademarks or registered trademarks mentioned are the properties of their respective owners.
Wonik Robotics's Allegro Hand is based on licensed technology developed by the Humanoid Robot Hand research group at the Korea Institute of Industrial Technology (KITECH).
Any references to the BHand Library or the Allegro Hand Motion and/or Grasping Library refer to a library of humanoid robotic hand grasping algorithms and motions developed and published by KITECH researchers.
J.-H. Bae, S.-W. Park, D. Kim, M.-H. Baeg, and S.-R. Oh, "A Grasp Strategy with the Geometric Centroid of a Groped Object Shape Derived from Contact Spots," Proc. of the 2012 IEEE Int. Conf. on Robotics and Automation (ICRA2012), pp. 3798-3804
Wiki maintained by Sean Yi <seanyi@wonikrobotics.com>
| Whos here now: Members 0 Guests 0 Bots & Crawlers 1 |