5. Joint PD Control Without Hand Library Revision as of 16:06, 14 November 2012 by Alexalspach (Talk | contribs)
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.
control_ERHand.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 control_ERHand::control_ERHand(rDC rdc) :rControlAlgorithmEx(rdc) , _cur_time(0) , _jdof(0) , _jid_test(0) , _test_mode(false) , _hand(NULL) , _is_left_hand(false) , _demo_mode(0) , _demo_start_time(0) , _RPS(0) , _zero(0) { } control_ERHand::~control_ERHand() { if (_hand) delete _hand; } 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) { const TCHAR* prop = NULL; prop = getProperty(_T("whichHand")); if (prop && _tcsicmp(prop, _T("right")) == 0) { _is_left_hand = false; _hand = bhCreateRightHand(); } else { _is_left_hand = true; _hand = bhCreateLeftHand(); } assert(_hand); _hand->SetTimeInterval(0.003); _jdof = JDOF; _arrangeJointDevices(); _remoteTP_BT = findDevice(_T("RemoteTP_BT")); _remoteTP_TCPIP = findDevice(_T("RemoteTP_TCPIP")); _q.resize(_jdof); _qdot.resize(_jdof); _torque.resize(_jdof); _q.zero(); _qdot.zero(); _torque.zero(); memset(_x, 0, sizeof(_x[0])*4); memset(_y, 0, sizeof(_y[0])*4); memset(_z, 0, sizeof(_z[0])*4); _sz2read_msgRTP = sizeof(MessageRemoteTP_t); _demo_q_des.resize(_jdof); _demo_q_des.zero(); } void control_ERHand::update(const rTime& t) { _cur_time = t; rControlAlgorithm::update(t); } void control_ERHand::setNominalSystem(const TCHAR* path, const TCHAR* aml, const HTransform& T0, const dVector& q0) { } void control_ERHand::setPeriod(const rTime& dT) { } void control_ERHand::_readDevices() { float val; for (int i=0; i<JDOF; i++) { if (_enc[i] != INVALID_RID) { readDeviceValue(_enc[i], &val, 4); _q[i] = (float)val; } } // 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() { 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_ERHand::_reflect() { } void control_ERHand::_compute(const double& t) { if (_hand) { if (_test_mode) { // TEST1: constant torque _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); }
control_ERHand.h
/* 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__ #include <list> #include "rControlAlgorithm/rControlAlgorithm.h" #include "BHand/BHand.h" #include "rDeviceERHandRemoteCmd.h" #define JDOF 16 class REXPORT control_ERHand : public rControlAlgorithmEx { public: control_ERHand(rDC rdc); ~control_ERHand(); 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 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(); void _servoOn(); void _servoOff(); private: rTime _cur_time; BHand* _hand; bool _is_left_hand; rID _motor[16]; rID _enc[16]; rID _remoteTP_BT; rID _remoteTP_TCPIP; dVector _q; dVector _qdot; dVector _torque; int _jdof; double _x[4]; double _y[4]; double _z[4]; double _fx[4]; double _fy[4]; double _fz[4]; double _f_norm[4]; // for testing each motor int _jid_test; bool _test_mode; // for Rock Paper Scissors int _RPS; int _zero; // for remote control MessageRemoteTP_t _msgRTP; int _sz2read_msgRTP; // for show off int _demo_mode; rTime _demo_start_time; dVector _demo_q_des; }; #endif
control_ERHandCMD.h
/* 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" #define BH_NONE (RCMD_USER + 0) #define BH_HOME (RCMD_GO_HOME) #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) #define BH_RPS (RCMD_USER + 51) #define BH_ALPHAMOD (RCMD_USER + 90) #define BH_TEST (RCMD_USER + 99) #define BH_ZERO (RCMD_USER + 100) #endif
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!
Accessing the Raw Encoder Data
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 |