allegroNode_sleep.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 #include <iostream>
00008 #include <boost/thread/thread.hpp>
00009 #include <boost/date_time.hpp>
00010 #include <boost/thread/locks.hpp>
00011 
00012 #include "ros/ros.h"
00013 #include "ros/service.h"
00014 #include "ros/service_server.h"
00015 #include "sensor_msgs/JointState.h"
00016 
00017 #include "BHand/BHand.h"
00018 #include "controlAllegroHand.h"
00019 
00020 #define JOINT_STATE_TOPIC "/allegro/joint_state"
00021 #define JOINT_CMD_TOPIC "/allegro/joint_cmd"
00022 
00023 double curr_position[DOF_JOINTS];
00024 double curr_torque[DOF_JOINTS];
00025 double desire_position[DOF_JOINTS];
00026 double desire_torque[DOF_JOINTS];
00027 
00028 double out_torque[DOF_JOINTS];
00029 
00030 
00031 boost::mutex *mutex;
00032 ros::Publisher joint_state_pub;
00033 ros::Subscriber joint_cmd_sub;
00034 sensor_msgs::JointState msgJoint;
00035 
00036 
00037 eMotionType gMotionType = eMotionType_NONE ;
00038 
00039 ros::Time tstart;
00040 ros::Time tend;
00041 
00042 
00043 void SetjointCallback(const sensor_msgs::JointState& msg)
00044 {
00045         // TODO check joint limits
00046 
00047 
00048         // copy
00049         mutex->lock();
00050         for(int i=0;i<DOF_JOINTS;i++) desire_position[i] = msg.position[i];
00051         mutex->unlock();
00052         //desire_position.Print();
00053         printf("%f",desire_position[0]);
00054 }
00055 
00056 
00057 int main(int argc, char** argv)
00058 {
00059         ros::init(argc, argv, "allegro");
00060         ros::Time::init();
00061         ros::NodeHandle nh;
00062         int lEmergencyStop = 0;
00063 
00064         mutex = new boost::mutex();
00065 
00066         joint_state_pub = nh.advertise<sensor_msgs::JointState>(JOINT_STATE_TOPIC, 3);
00067         joint_cmd_sub = nh.subscribe(JOINT_CMD_TOPIC, 3, SetjointCallback);
00068         msgJoint.position.resize(DOF_JOINTS);
00069 
00070 
00071         // initialize BHand controller
00072         BHand lBHand(eHandType_Right);
00073         lBHand.SetTimeInterval(ALLEGRO_CONTROL_TIME_INTERVAL);
00074 
00075         ros::Rate rate(1./ALLEGRO_CONTROL_TIME_INTERVAL);
00076         int frame =0;
00077 
00078         // initialize device
00079         controlAllegroHand *canDevice;
00080         canDevice = new controlAllegroHand();
00081         canDevice->init();
00082         usleep(3000);
00083 
00084         for(int i=0; i<16; i++) desire_torque[i] = 0.0;
00085 
00086         tstart = ros::Time::now();
00087         double dt;
00088 
00089         bool lIsBegin = false;
00090         FILE *fid;
00091         fid = fopen("./allegro_log.txt", "w+");
00092         
00093         FILE *fidTime;
00094         fidTime = fopen("./allegro_Timelog.txt", "w+");
00095 
00096         while(ros::ok())
00097         {
00098 
00099                 tend = ros::Time::now();
00100                 dt = 1e-9*(tend - tstart).nsec;
00101                 tstart = tend;
00102 
00103                 canDevice->setTorque(desire_torque);
00104                 lEmergencyStop = canDevice->update();
00105                 canDevice->getJointInfo(curr_position, curr_torque);
00106 
00107 
00108 
00109 
00110                 for(int i=0; i<DOF_JOINTS; i++ ) fprintf(fid, "%lf ", desire_position[i]);
00111                 for(int i=0; i<DOF_JOINTS; i++ ) fprintf(fid, "%lf ", curr_position[i]);
00112                 for(int i=0; i<DOF_JOINTS; i++ ) fprintf(fid, "%lf ", desire_torque[i]);
00113                 fprintf(fid, "\n");
00114                 
00115                 double secs =ros::Time::now().toSec();
00116                 fprintf(fidTime, "%lf ", secs);
00117                 fprintf(fidTime, "\n");
00118 
00119                 if( lEmergencyStop < 0 )
00120                 {
00121                         std::cout << "emergency stop " << std::endl;
00122                         break;
00123                 }
00124 
00125                 // compute torque
00126                 lBHand.SetJointPosition(curr_position);
00127                 if( lIsBegin == false ){
00128                         if(frame > 10){
00129                                 mutex->lock();
00130                                 for(int i=0; i<DOF_JOINTS; i++) desire_position[i] = curr_position[i];
00131                                 mutex->unlock();
00132 
00133                                 lIsBegin = true;
00134                         }
00135 
00136                         lBHand.SetMotionType(eMotionType_JOINT_PD);
00137                         lBHand.UpdateControl((double)frame*ALLEGRO_CONTROL_TIME_INTERVAL);
00138                         for(int i=0; i<DOF_JOINTS; i++) desire_torque[i] = 0.0;
00139                 }
00140                 else{
00141                         mutex->lock();
00142                         lBHand.SetJointDesiredPosition(desire_position);
00143                         mutex->unlock();
00144 
00145                         lBHand.SetMotionType(eMotionType_JOINT_PD);
00146 
00147                         lBHand.UpdateControl((double)frame*ALLEGRO_CONTROL_TIME_INTERVAL);
00148                         lBHand.GetJointTorque(out_torque);
00149 
00150                         for(int i=0; i<DOF_JOINTS; i++) desire_torque[i] = out_torque[i];
00151                         for(int i=0; i<DOF_JOINTS; i++) msgJoint.position[i] = curr_position[i];
00152                         joint_state_pub.publish(msgJoint);
00153                 }
00154                 frame++;
00155 
00156                 ros::spinOnce();
00157                 rate.sleep();
00158         }
00159 
00160         fclose(fid);
00161         nh.shutdown();
00162         delete canDevice;
00163         printf("bye\n");
00164 
00165         return 0;
00166 }
00167 
00168 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines


AllegroHand
Author(s): Alex Alspach (SimLab), Seungsu Kim (EPFL)
autogenerated on Thu Mar 21 2013 21:24:05