allegroNode.cpp
Go to the documentation of this file.
00001 /*
00002  * allegroNode.cpp
00003  *
00004  *  Created on: Nov 14, 2012
00005  *  Authors: Alex ALSPACH, Seungsu KIM
00006  */
00007  
00008 // GRASP LIBRARY INTERFACE
00009 // Using  timer callback  
00010  
00011 #include <iostream>
00012 #include <boost/thread/thread.hpp>
00013 #include <boost/date_time.hpp>
00014 #include <boost/thread/locks.hpp>
00015 
00016 #include "ros/ros.h"
00017 #include "ros/service.h"
00018 #include "ros/service_server.h"
00019 #include "sensor_msgs/JointState.h"
00020 #include "std_msgs/String.h"
00021 
00022 #include <stdio.h>
00023 #include <iostream>
00024 #include <string>
00025 
00026 #include "BHand/BHand.h"
00027 #include "controlAllegroHand.h"
00028 
00029 // Topics
00030 #define JOINT_STATE_TOPIC "/allegroHand/joint_states"
00031 #define JOINT_CMD_TOPIC "/allegroHand/joint_cmd"
00032 #define LIB_CMD_TOPIC "/allegroHand/lib_cmd"
00033 
00034 
00035 double desired_position[DOF_JOINTS]                     = {0.0};
00036 double current_position[DOF_JOINTS]                     = {0.0};
00037 double previous_position[DOF_JOINTS]                    = {0.0};
00038 double current_position_filtered[DOF_JOINTS]    = {0.0};
00039 double previous_position_filtered[DOF_JOINTS]   = {0.0};
00040 
00041 double current_velocity[DOF_JOINTS]                     = {0.0};
00042 double previous_velocity[DOF_JOINTS]                    = {0.0};
00043 double current_velocity_filtered[DOF_JOINTS]    = {0.0};
00044 
00045 double desired_torque[DOF_JOINTS]                               = {0.0};
00046 
00047 std::string  lib_cmd;
00048 
00049 
00050 std::string jointNames[DOF_JOINTS]      = {    "joint_0.0",    "joint_1.0",    "joint_2.0",   "joint_3.0" , 
00051                                                                                    "joint_4.0",    "joint_5.0",    "joint_6.0",   "joint_7.0" , 
00052                                                                                    "joint_8.0",    "joint_9.0",    "joint_10.0",  "joint_11.0", 
00053                                                                                    "joint_12.0",   "joint_13.0",   "joint_14.0",  "joint_15.0" };
00054                                                                                   
00055 int frame = 0;
00056 
00057 // Flags
00058 bool lIsBegin = false;
00059 bool pdControl = true;
00060 int lEmergencyStop = 0;
00061 
00062 boost::mutex *mutex;
00063 
00064 // ROS Messages
00065 ros::Publisher joint_state_pub;
00066 ros::Subscriber joint_cmd_sub;
00067 ros::Subscriber lib_cmd_sub;
00068 sensor_msgs::JointState msgJoint;
00069 
00070 // ROS Time
00071 ros::Time tstart;
00072 ros::Time tnow;
00073 double secs;
00074 double dt;
00075 
00076 // Initialize BHand 
00077 eMotionType gMotionType = eMotionType_NONE ;
00078 BHand* pBHand = NULL;
00079 //BHand lBHand(eHandType_Right);
00080 
00081 // Initialize CAN device                
00082 controlAllegroHand* canDevice;
00083 
00084 
00085 
00086 // Called when a desired joint position message is received
00087 void SetjointCallback(const sensor_msgs::JointState& msg)
00088 {
00089         // TODO check joint limits
00090         pdControl=true;
00091         
00092         mutex->lock();
00093         for(int i=0;i<DOF_JOINTS;i++) desired_position[i] = msg.position[i];
00094         mutex->unlock();
00095         
00096         pBHand->SetMotionType(eMotionType_JOINT_PD);
00097 }
00098 
00099 void libCmdCallback(const std_msgs::String::ConstPtr& msg)
00100 {
00101         ROS_INFO("CTRL: Heard: [%s]", msg->data.c_str());
00102         lib_cmd = msg->data.c_str();
00103 
00104         // If PD Control is commanded, pdControl is set true
00105         // so that the controll loop has access to desired position.
00106         if (lib_cmd.compare("pdControl") == 0)
00107         {
00108                 pBHand->SetMotionType(eMotionType_JOINT_PD);
00109                 pdControl = true;
00110         }    
00111         else
00112         pdControl = false;  
00113         
00114         if (lib_cmd.compare("home") == 0) 
00115                 pBHand->SetMotionType(eMotionType_HOME);
00116     
00117         if (lib_cmd.compare("ready") == 0) 
00118                 pBHand->SetMotionType(eMotionType_READY);
00119         
00120         if (lib_cmd.compare("grasp_3") == 0) 
00121                 pBHand->SetMotionType(eMotionType_GRASP_3);
00122         
00123         if (lib_cmd.compare("grasp_4") == 0) 
00124                 pBHand->SetMotionType(eMotionType_GRASP_4);
00125     
00126         if (lib_cmd.compare("pinch_it") == 0) 
00127                 pBHand->SetMotionType(eMotionType_PINCH_IT);
00128         
00129         if (lib_cmd.compare("pinch_mt") == 0) 
00130                 pBHand->SetMotionType(eMotionType_PINCH_MT);     
00131         
00132         if (lib_cmd.compare("envelop") == 0) 
00133                 pBHand->SetMotionType(eMotionType_ENVELOP); 
00134         
00135         if (lib_cmd.compare("off") == 0) 
00136                 pBHand->SetMotionType(eMotionType_NONE);
00137         
00138         if (lib_cmd.compare("save") == 0) 
00139                 for(int i=0; i<DOF_JOINTS; i++) desired_position[i] = current_position[i];
00140                 
00141 /*
00142 eMotionType_NONE,                               // motor power off
00143 eMotionType_HOME,                               // go to home position
00144 eMotionType_READY,                              // ready position for grasping
00145 eMotionType_GRASP_3,                    // grasping using 3 fingers
00146 eMotionType_GRASP_4,                    // grasping using 4 fingers
00147 eMotionType_PINCH_IT,                   // pinching using index finger and thumb
00148 eMotionType_PINCH_MT,                   // pinching using middle finger and thumb
00149 eMotionType_ENVELOP,                    // enveloping
00150 eMotionType_JOINT_PD,                   // joint pd control
00151 eMotionType_OBJECT_MOVING,              //
00152 eMotionType_PRE_SHAPE,                  //
00153 */      
00154         
00155 }
00156 
00157 
00158 void timerCallback(const ros::TimerEvent& event)
00159 {
00160         // Calculate loop time;
00161         tnow = ros::Time::now();
00162         dt = 1e-9*(tnow - tstart).nsec;
00163         tstart = tnow;
00164                 
00165         // save last iteration info
00166         for(int i=0; i<DOF_JOINTS; i++)
00167         {
00168                 previous_position[i] = current_position[i];
00169                 previous_position_filtered[i] = current_position_filtered[i];
00170                 previous_velocity[i] = current_velocity[i];
00171         }
00172                 
00173         /* ================================== 
00174          =        CAN COMMUNICATION         =   
00175          ================================== */
00176         canDevice->setTorque(desired_torque);
00177         lEmergencyStop = canDevice->update();
00178         canDevice->getJointInfo(current_position);
00179         
00180         
00181         
00182         /* ================================== 
00183          =         LOWPASS FILTERING        =   
00184          ================================== */
00185         for(int i=0; i<DOF_JOINTS; i++)    
00186         {
00187                 current_position_filtered[i] = (0.6*current_position_filtered[i]) + (0.198*previous_position[i]) + (0.198*current_position[i]);
00188                 current_velocity[i] = (current_position_filtered[i] - previous_position_filtered[i]) / dt;
00189                 current_velocity_filtered[i] = (0.6*current_velocity_filtered[i]) + (0.198*previous_velocity[i]) + (0.198*current_velocity[i]);
00190         }       
00191         
00192                                 
00193         if( lEmergencyStop < 0 )
00194         {
00195                 // Stop program when Allegro Hand is switched off
00196                 //printf("\n\n\nEMERGENCY STOP.\n\n");
00197                 ROS_ERROR("\n\nAllegro Hand Node is Shutting Down! (Emergency Stop)\n");
00198                 ros::shutdown();
00199         }
00200 
00201         // compute control torque using Bhand library
00202         pBHand->SetJointPosition(current_position_filtered);
00203         
00204         // Run on FIRST iteration (sets PD control of intitial position)
00205         if( lIsBegin == false ){
00206                 if(frame > 10){
00207                         mutex->lock();
00208                         for(int i=0; i<DOF_JOINTS; i++) desired_position[i] = current_position[i];
00209                         mutex->unlock();
00210 
00211                         lIsBegin = true;
00212                         }
00213                         
00214                         // Start joint position control (Bhand)
00215                         pBHand->SetMotionType(eMotionType_JOINT_PD);
00216                         pBHand->UpdateControl((double)frame*ALLEGRO_CONTROL_TIME_INTERVAL);
00217                         for(int i=0; i<DOF_JOINTS; i++) desired_torque[i] = 0.0;
00218                 }
00219                 else{                   
00220                 
00221                         // BHAND Communication
00222                         // desired joint positions are obtained from subscriber "joint_cmd_sub"
00223                         // or maintatined as the initial positions from program start (PD control)
00224                         // Also, other motions/grasps can be executed (envoked via "lib_cmd_sub")
00225                         
00226                         // Desired position only necessary if in PD Control mode
00227                         if (pdControl==true)
00228                                 pBHand->SetJointDesiredPosition(desired_position);
00229                         
00230                         // BHand lib control updated with time stamp
00231                         pBHand->UpdateControl((double)frame*ALLEGRO_CONTROL_TIME_INTERVAL);
00232                         
00233                         // Necessary torque obtained from Bhand lib
00234                         pBHand->GetJointTorque(desired_torque);
00235                         
00236                         // Calculate joint velocity
00237                         for(int i=0; i<DOF_JOINTS; i++)
00238                                 current_velocity[i] = (current_position[i] - previous_position[i])/dt;
00239                         
00240                         // current position, velocity and effort (torque) published
00241                         msgJoint.header.stamp                                                                   = tnow;
00242                         for(int i=0; i<DOF_JOINTS; i++) msgJoint.position[i]    = current_position_filtered[i];
00243                         for(int i=0; i<DOF_JOINTS; i++) msgJoint.velocity[i]    = current_velocity_filtered[i];
00244                         for(int i=0; i<DOF_JOINTS; i++) msgJoint.effort[i]              = desired_torque[i];
00245                         joint_state_pub.publish(msgJoint);
00246                 }
00247                 
00248                 frame++;
00249 } 
00250 // end timerCallback
00251 
00252 
00253 
00254 
00255 
00256 
00257 
00258 
00259 int main(int argc, char** argv)
00260 {
00261         using namespace std;
00262         
00263         ros::init(argc, argv, "allegro_hand_core_grasp");
00264         ros::Time::init();
00265         
00266         ros::NodeHandle nh;
00267 
00268         // Setup timer callback
00269         ros::Timer timer = nh.createTimer(ros::Duration(0.003), timerCallback);
00270 
00271         mutex = new boost::mutex();
00272 
00273         // Publisher and Subscribers
00274         joint_state_pub = nh.advertise<sensor_msgs::JointState>(JOINT_STATE_TOPIC, 3);
00275         joint_cmd_sub = nh.subscribe(JOINT_CMD_TOPIC, 3, SetjointCallback);
00276         lib_cmd_sub = nh.subscribe(LIB_CMD_TOPIC, 1, libCmdCallback);
00277         
00278         // Create arrays 16 long for each of the four joint state components
00279         msgJoint.position.resize(DOF_JOINTS);
00280         msgJoint.velocity.resize(DOF_JOINTS);
00281         msgJoint.effort.resize(DOF_JOINTS);
00282         msgJoint.name.resize(DOF_JOINTS);
00283 
00284         // Joint names (for use with joint_state_publisher GUI - matches URDF)
00285         for(int i=0; i<DOF_JOINTS; i++) msgJoint.name[i] = jointNames[i];       
00286 
00287         
00288         // Get Allegro Hand information from parameter server
00289         // This information is found in the Hand-specific "zero.yaml" file from the allegro_hand_description package    
00290         string robot_name, whichHand, manufacturer, origin, serial;
00291         double version;
00292         ros::param::get("~hand_info/robot_name",robot_name);
00293         ros::param::get("~hand_info/which_hand",whichHand);
00294         ros::param::get("~hand_info/manufacturer",manufacturer);
00295         ros::param::get("~hand_info/origin",origin);
00296         ros::param::get("~hand_info/serial",serial);
00297         ros::param::get("~hand_info/version",version);
00298 
00299         // Initialize BHand controller
00300         if (whichHand.compare("left") == 0)
00301         {
00302                 pBHand = new BHand(eHandType_Left);
00303                 ROS_WARN("CTRL: Left Allegro Hand controller initialized.");
00304         }
00305         else
00306         {
00307                 pBHand = new BHand(eHandType_Right);
00308                 ROS_WARN("CTRL: Right Allegro Hand controller initialized.");
00309         }
00310         pBHand->SetTimeInterval(ALLEGRO_CONTROL_TIME_INTERVAL);
00311 
00312         // Initialize CAN device
00313         canDevice = new controlAllegroHand();
00314         canDevice->init();
00315         usleep(3000);
00316         
00317         // Dump Allegro Hand information to the terminal        
00318         cout << endl << endl << robot_name << " v" << version << endl << serial << " (" << whichHand << ")" << endl << manufacturer << endl << origin << endl << endl;
00319         
00320 
00321         // Initialize torque at zero
00322         for(int i=0; i<16; i++) desired_torque[i] = 0.0;
00323 
00324         // Set to false for first iteration of control loop (sets PD control at start pose)
00325         lIsBegin = false;
00326 
00327         // Start ROS time
00328         tstart = ros::Time::now();
00329 
00330         // Starts control loop, message pub/subs and all other callbacks
00331         ros::spin();                    
00332 
00333         // Prompt user to press enter to quit node for viewing failed launch or system after CAN error
00334         //cout << "Press Enter to Continue";
00335         //cin.ignore();
00336 
00337         // Clean shutdown: shutdown node, shutdown can, be polite.
00338         nh.shutdown();
00339         delete canDevice;
00340         delete pBHand;
00341         //printf("\nBye.\n");
00342         printf("\nAllegro Hand Node has been shut down. Bye!\n\n");
00343         return 0;
00344 }
00345 
00346 


allegro_hand_core_grasp
Author(s): Alex Alspach (SimLab), Seungsu Kim (EPFL)
autogenerated on Mon Jan 6 2014 11:02:56