allegroNode.cpp
Go to the documentation of this file.
00001 /*
00002  * allegroNode.cpp
00003  *
00004  *  Created on: Nov 14, 2012
00005  *      Author: Seungsu KIM
00006  */
00007  
00008  // Add something like, if the time is bigger than nominal (0.003)+tolerance, sprit it out to the terminal. Also, consider dropping the file writing all together and saving bag file ros style.
00009  //Make a launch file that will launch the bag file saver along with allegro. What else is the launch file good for? time to start integrating the kinect???
00010  
00011  
00012 #include <iostream>
00013 #include <boost/thread/thread.hpp>
00014 #include <boost/date_time.hpp>
00015 #include <boost/thread/locks.hpp>
00016 
00017 #include "ros/ros.h"
00018 #include "ros/service.h"
00019 #include "ros/service_server.h"
00020 #include "sensor_msgs/JointState.h"
00021 
00022 #include "BHand/BHand.h"
00023 #include "controlAllegroHand.h"
00024 
00025 #define JOINT_STATE_TOPIC "/allegro/joint_state"
00026 #define JOINT_CMD_TOPIC "/allegro/joint_cmd"
00027 
00028 double curr_position[DOF_JOINTS];
00029 double curr_position_pre[DOF_JOINTS];
00030 double curr_velocity[DOF_JOINTS];
00031 double curr_torque[DOF_JOINTS];
00032 double desire_position[DOF_JOINTS];
00033 double desire_torque[DOF_JOINTS];
00034 
00035 double secs;
00036 
00037 double out_torque[DOF_JOINTS];
00038 
00039 int frame = 0;
00040 int lEmergencyStop = 0;
00041 
00042 double dt;
00043 
00044 bool lIsBegin = false;
00045 int kill = false;       
00046 
00047 FILE *fid;
00048 FILE *fidTime;
00049 
00050 boost::mutex *mutex;
00051 
00052 ros::Publisher joint_state_pub;
00053 ros::Subscriber joint_cmd_sub;
00054 sensor_msgs::JointState msgJoint;
00055 
00056 ros::Time tstart;
00057 ros::Time tnow;
00058 
00059 //      ros::NodeHandle nh;
00060 
00061 // initialize BHand 
00062 eMotionType gMotionType = eMotionType_NONE ;
00063 BHand lBHand(eHandType_Right);
00064         
00065 controlAllegroHand *canDevice;
00066 
00067 void SetjointCallback(const sensor_msgs::JointState& msg)
00068 {
00069         // TODO check joint limits
00070 
00071 
00072         // copy
00073         mutex->lock();
00074         for(int i=0;i<DOF_JOINTS;i++) desire_position[i] = msg.position[i];
00075         mutex->unlock();
00076         //desire_position.Print();
00077         printf("%f",desire_position[0]);
00078 }
00079 
00080 void timerCallback(const ros::TimerEvent& event)
00081 {
00082         //fid = fopen("./allegro_log.txt", "w+");
00083         //fidTime = fopen("./allegro_Timelog.txt", "w+");
00084 
00085                 //printf("Timer Callback\n\n");
00086                 tnow = ros::Time::now();
00087                 dt = (tnow - tstart).nsec; //1e-9*(tnow - tstart).nsec;
00088                 tstart = tnow;
00089                 
00090                 // save last joint position for velocity calc
00091                 for(int i=0; i<DOF_JOINTS; i++) curr_position_pre[i] = curr_position[i];
00092                 
00094                 canDevice->setTorque(desire_torque);
00095                 lEmergencyStop = canDevice->update();
00096                 canDevice->getJointInfo(curr_position, curr_torque);
00098                                 
00099                 if( lEmergencyStop < 0 )
00100                 {
00101                         // stop program when Allegro Hand is switched off
00102                         std::cout << "emergency stop " << std::endl;
00103                         kill = true;
00104                 }
00105                 
00106                 // print to file (doesnt work)
00107                 for(int i=0; i<DOF_JOINTS; i++ ) fprintf(fid, "%lf ", desire_position[i]);
00108                 for(int i=0; i<DOF_JOINTS; i++ ) fprintf(fid, "%lf ", curr_position[i]);
00109                 for(int i=0; i<DOF_JOINTS; i++ ) fprintf(fid, "%lf ", desire_torque[i]);
00110                 fprintf(fid, "%lf\n",dt);
00111                 
00112 
00113                 // compute torque using Bhand library
00114                 lBHand.SetJointPosition(curr_position);
00115                 if( lIsBegin == false ){
00116                 // set desired joint position to initial position when program is initialized
00117                         if(frame > 10){
00118                                 mutex->lock();
00119                                 for(int i=0; i<DOF_JOINTS; i++) desire_position[i] = curr_position[i];
00120                                 mutex->unlock();
00121 
00122                                 lIsBegin = true;
00123                         }
00124                         // start joint position control (Bhand)
00125                         lBHand.SetMotionType(eMotionType_JOINT_PD);
00126                         lBHand.UpdateControl((double)frame*ALLEGRO_CONTROL_TIME_INTERVAL);
00127                         for(int i=0; i<DOF_JOINTS; i++) desire_torque[i] = 0.0;
00128                 }
00129                 else{
00131                         // desired position is either commanded to the joint state subscriber
00132                         // or maintatined as the initial position from program start
00133                         mutex->lock();
00134                         // desired position sent to Bhand lib
00135                         lBHand.SetJointDesiredPosition(desire_position);
00136                         mutex->unlock();
00137 
00138                         lBHand.SetMotionType(eMotionType_JOINT_PD);
00139                         lBHand.UpdateControl((double)frame*ALLEGRO_CONTROL_TIME_INTERVAL);
00140                         // necessary torque obtained from Bhand lib
00141                         lBHand.GetJointTorque(out_torque);
00142                         for(int i=0; i<DOF_JOINTS; i++) desire_torque[i] = out_torque[i];
00144                         
00145                         // calculate current velocity
00146                         for(int i=0; i<DOF_JOINTS; i++) curr_velocity[i] = (curr_position[i] - curr_position_pre[i])/dt;
00147                         
00148                         // current position, velocity and effort (torque) published
00149                         msgJoint.header.stamp = tnow;
00150                         for(int i=0; i<DOF_JOINTS; i++) msgJoint.position[i] = curr_position[i];
00151                         for(int i=0; i<DOF_JOINTS; i++) msgJoint.velocity[i] = curr_velocity[i];
00152                         for(int i=0; i<DOF_JOINTS; i++) msgJoint.effort[i] = desire_torque[i];
00153                         joint_state_pub.publish(msgJoint);
00154                 }
00155                 frame++;
00156 
00157                 //ros::spinOnce();
00158                 //rate.sleep();
00159 }
00160 
00161 int main(int argc, char** argv)
00162 {
00163         fid = fopen("./allegro_log.txt", "w+");
00164         fidTime = fopen("./allegro_Timelog.txt", "w+");
00165 
00166         ros::init(argc, argv, "allegro");
00167         ros::Time::init();
00168         
00169         ros::NodeHandle nh;
00170 
00171         ros::Timer timer = nh.createTimer(ros::Duration(0.003), timerCallback);
00172 
00173         mutex = new boost::mutex();
00174 
00175         joint_state_pub = nh.advertise<sensor_msgs::JointState>(JOINT_STATE_TOPIC, 3);
00176         joint_cmd_sub = nh.subscribe(JOINT_CMD_TOPIC, 3, SetjointCallback);
00177         msgJoint.position.resize(DOF_JOINTS);
00178         msgJoint.velocity.resize(DOF_JOINTS);
00179         msgJoint.effort.resize(DOF_JOINTS);
00180         msgJoint.name.resize(DOF_JOINTS);
00181         
00182         msgJoint.name[0]  = "00";
00183         msgJoint.name[1]  = "01";
00184         msgJoint.name[2]  = "02";
00185         msgJoint.name[3]  = "03";
00186         msgJoint.name[4]  = "10";
00187         msgJoint.name[5]  = "11";
00188         msgJoint.name[6]  = "12";
00189         msgJoint.name[7]  = "13";
00190         msgJoint.name[8]  = "20";
00191         msgJoint.name[9]  = "21";
00192         msgJoint.name[10] = "22";
00193         msgJoint.name[11] = "23";
00194         msgJoint.name[12] = "30";
00195         msgJoint.name[13] = "31";
00196         msgJoint.name[14] = "32";
00197         msgJoint.name[15] = "33";
00198 
00199         // initialize BHand controller
00200         //BHand lBHand(eHandType_Right);
00201         lBHand.SetTimeInterval(ALLEGRO_CONTROL_TIME_INTERVAL);
00202 
00203         //ros::Rate rate(1./ALLEGRO_CONTROL_TIME_INTERVAL);
00204 
00205         //controlAllegroHand *canDevice;
00206         // initialize device
00207 
00208         printf("CAN open1\n\n");
00209         canDevice = new controlAllegroHand();
00210         printf("CAN open2\n\n");
00211         canDevice->init();
00212         printf("CAN open3\n\n");
00213         usleep(3000);
00214         
00215         printf("CAN open4\n\n");
00216 
00217         for(int i=0; i<16; i++) desire_torque[i] = 0.0;
00218 
00219         tstart = ros::Time::now();
00220 
00221         lIsBegin = false;
00222 
00223         //while(kill==false)
00224         //{
00225         //}
00226         
00227         while(kill==false)
00228         {
00229                         ros::spinOnce();
00230         }
00231                         
00232 
00233 //if (kill==true)
00234 //{
00235         fclose(fid);
00236         fclose(fidTime);
00237         nh.shutdown();
00238         delete canDevice;
00239         printf("bye\n");
00240 
00241         return 0;
00242 //}
00243 }
00244 
00245 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


AllegroHand
Author(s): Alex Alspach (SimLab), Seungsu Kim (EPFL)
autogenerated on Fri Mar 22 2013 04:47:25