00001
00002
00003
00004
00005
00006
00007
00008
00009
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 #include "std_msgs/Float32.h"
00022
00023 #include <stdio.h>
00024 #include <math.h>
00025 #include <iostream>
00026 #include <string>
00027
00028 #include "controlAllegroHand.h"
00029
00030
00031 #define RADIANS_TO_DEGREES(radians) ((radians) * (180.0 / M_PI))
00032 #define DEGREES_TO_RADIANS(angle) ((angle) / 180.0 * M_PI)
00033
00034
00035
00036 #define JOINT_STATE_TOPIC "/allegroHand/joint_states"
00037 #define JOINT_CMD_TOPIC "/allegroHand/joint_cmd"
00038 #define EXT_CMD_TOPIC "/allegroHand/external_cmd"
00039
00040 double current_position[DOF_JOINTS] = {0.0};
00041 double previous_position[DOF_JOINTS] = {0.0};
00042
00043 double current_position_filtered[DOF_JOINTS] = {0.0};
00044 double previous_position_filtered[DOF_JOINTS] = {0.0};
00045
00046 double current_velocity[DOF_JOINTS] = {0.0};
00047 double previous_velocity[DOF_JOINTS] = {0.0};
00048 double current_velocity_filtered[DOF_JOINTS] = {0.0};
00049
00050 double desired_position[DOF_JOINTS] = {0.0};
00051 double desired_torque[DOF_JOINTS] = {0.0};
00052
00053 double k_p[DOF_JOINTS] = { 600.0, 600.0, 600.0, 1000.0,
00054 600.0, 600.0, 600.0, 1000.0,
00055 600.0, 600.0, 600.0, 1000.0,
00056 1000.0, 1000.0, 1000.0, 600.0 };
00057
00058 double k_d[DOF_JOINTS] = { 15.0, 20.0, 15.0, 15.0,
00059 15.0, 20.0, 15.0, 15.0,
00060 15.0, 20.0, 15.0, 15.0,
00061 30.0, 20.0, 20.0, 15.0 };
00062
00063
00064 double home_pose[DOF_JOINTS] = { 0.0, -10.0, 45.0, 45.0,
00065 0.0, -10.0, 45.0, 45.0,
00066 5.0, -5.0, 50.0, 45.0,
00067 60.0, 25.0, 15.0, 45.0 };
00068
00069
00070 std::string pGainParams[DOF_JOINTS] = { "~gains_pd/p/j00", "~gains_pd/p/j01", "~gains_pd/p/j02", "~gains_pd/p/j03",
00071 "~gains_pd/p/j10", "~gains_pd/p/j11", "~gains_pd/p/j12", "~gains_pd/p/j13",
00072 "~gains_pd/p/j20", "~gains_pd/p/j21", "~gains_pd/p/j22", "~gains_pd/p/j23",
00073 "~gains_pd/p/j30", "~gains_pd/p/j31", "~gains_pd/p/j32", "~gains_pd/p/j33"};
00074
00075 std::string dGainParams[DOF_JOINTS] = { "~gains_pd/d/j00", "~gains_pd/d/j01", "~gains_pd/d/j02", "~gains_pd/d/j03",
00076 "~gains_pd/d/j10", "~gains_pd/d/j11", "~gains_pd/d/j12", "~gains_pd/d/j13",
00077 "~gains_pd/d/j20", "~gains_pd/d/j21", "~gains_pd/d/j22", "~gains_pd/d/j23",
00078 "~gains_pd/d/j30", "~gains_pd/d/j31", "~gains_pd/d/j32", "~gains_pd/d/j33"};
00079
00080 std::string initialPosition[DOF_JOINTS] = { "~initial_position/j00", "~initial_position/j01", "~initial_position/j02", "~initial_position/j03",
00081 "~initial_position/j10", "~initial_position/j11", "~initial_position/j12", "~initial_position/j13",
00082 "~initial_position/j20", "~initial_position/j21", "~initial_position/j22", "~initial_position/j23",
00083 "~initial_position/j30", "~initial_position/j31", "~initial_position/j32", "~initial_position/j33"};
00084
00085
00086 std::string jointNames[DOF_JOINTS] = { "joint_0.0", "joint_1.0", "joint_2.0", "joint_3.0" ,
00087 "joint_4.0", "joint_5.0", "joint_6.0", "joint_7.0" ,
00088 "joint_8.0", "joint_9.0", "joint_10.0", "joint_11.0",
00089 "joint_12.0", "joint_13.0", "joint_14.0", "joint_15.0" };
00090
00091
00092 int frame = 0;
00093
00094
00095 int lEmergencyStop = 0;
00096
00097 boost::mutex *mutex;
00098
00099
00100 ros::Publisher joint_state_pub;
00101 ros::Subscriber joint_cmd_sub;
00102 ros::Subscriber external_cmd_sub;
00103 sensor_msgs::JointState msgJoint;
00104 std::string ext_cmd;
00105
00106
00107 ros::Time tstart;
00108 ros::Time tnow;
00109 double secs;
00110 double dt;
00111
00112
00113 controlAllegroHand *canDevice;
00114
00115
00116
00117
00118
00119
00120
00121
00122 void SetjointCallback(const sensor_msgs::JointState& msg)
00123 {
00124 mutex->lock();
00125 for(int i=0;i<DOF_JOINTS;i++) desired_position[i] = msg.position[i];
00126 mutex->unlock();
00127 }
00128
00129
00130
00131 void extCmdCallback(const std_msgs::String::ConstPtr& msg)
00132 {
00133 ROS_INFO("CTRL: Processing external command");
00134 ext_cmd = msg->data.c_str();
00135
00136
00137 if (ext_cmd.compare("comparisonString") == 0)
00138 {
00139
00140 }
00141 else
00142 {
00143
00144 }
00145 }
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169 int main(int argc, char** argv)
00170 {
00171
00172 using namespace std;
00173
00174 ros::init(argc, argv, "allegro_hand_core");
00175 ros::Time::init();
00176
00177 ros::NodeHandle nh;
00178
00179
00180
00181 ros::Rate rate(1./ALLEGRO_CONTROL_TIME_INTERVAL);
00182
00183 mutex = new boost::mutex();
00184
00185
00186 joint_state_pub = nh.advertise<sensor_msgs::JointState>(JOINT_STATE_TOPIC, 3);
00187 joint_cmd_sub = nh.subscribe(JOINT_CMD_TOPIC, 3, SetjointCallback);
00188 external_cmd_sub = nh.subscribe(EXT_CMD_TOPIC, 1, extCmdCallback);
00189
00190
00191 msgJoint.position.resize(DOF_JOINTS);
00192 msgJoint.velocity.resize(DOF_JOINTS);
00193 msgJoint.effort.resize(DOF_JOINTS);
00194 msgJoint.name.resize(DOF_JOINTS);
00195
00196
00197
00198 for(int i=0; i<DOF_JOINTS; i++) msgJoint.name[i] = jointNames[i];
00199
00200
00201
00202
00203 string robot_name, whichHand, manufacturer, origin, serial;
00204 double version;
00205 ros::param::get("~hand_info/robot_name",robot_name);
00206 ros::param::get("~hand_info/which_hand",whichHand);
00207 ros::param::get("~hand_info/manufacturer",manufacturer);
00208 ros::param::get("~hand_info/origin",origin);
00209 ros::param::get("~hand_info/serial",serial);
00210 ros::param::get("~hand_info/version",version);
00211
00212
00213
00214 if (ros::param::has("~gains_pd"))
00215 {
00216 ROS_INFO("\n\nCTRL: PD gains loaded from param server.\n");
00217 for(int i=0; i<DOF_JOINTS; i++)
00218 {
00219 ros::param::get(pGainParams[i], k_p[i]);
00220 ros::param::get(dGainParams[i], k_d[i]);
00221
00222 }
00223
00224 }
00225 else
00226 {
00227
00228 ROS_WARN("\n\nCTRL: PD gains not loaded.\nCheck launch file is loading /parameters/gains_pd.yaml\nloading default PD gains...\n");
00229 }
00230
00231
00232
00233 if (ros::param::has("~initial_position"))
00234 {
00235 ROS_INFO("\n\nCTRL: Initial Pose loaded from param server.\n");
00236 for(int i=0; i<DOF_JOINTS; i++)
00237 {
00238 ros::param::get(initialPosition[i], desired_position[i]);
00239 desired_position[i] = DEGREES_TO_RADIANS(desired_position[i]);
00240 }
00241 }
00242 else
00243 {
00244 ROS_WARN("\n\nCTRL: Initial postion not loaded.\nCheck launch file is loading /parameters/initial_position.yaml\nloading Home position instead...\n");
00245
00246 for(int i=0; i<DOF_JOINTS; i++) desired_position[i] = DEGREES_TO_RADIANS(home_pose[i]);
00247 }
00248
00249
00250
00251 canDevice = new controlAllegroHand();
00252 canDevice->init();
00253 usleep(3000);
00254
00255
00256 cout << endl << endl << robot_name << " v" << version << endl << serial << " (" << whichHand << ")" << endl << manufacturer << endl << origin << endl << endl;
00257
00258
00259 tstart = ros::Time::now();
00260
00261
00262
00263 while(ros::ok())
00264 {
00265
00266
00267
00268 tnow = ros::Time::now();
00269 dt = 1e-9*(tnow - tstart).nsec;
00270 tstart = tnow;
00271
00272
00273
00274
00275
00276
00277 for(int i=0; i<DOF_JOINTS; i++)
00278 {
00279 previous_position[i] = current_position[i];
00280 previous_position_filtered[i] = current_position_filtered[i];
00281 previous_velocity[i] = current_velocity[i];
00282 }
00283
00284
00285
00286
00287
00288 canDevice->setTorque(desired_torque);
00289 lEmergencyStop = canDevice->update();
00290 canDevice->getJointInfo(current_position);
00291
00292
00293
00294
00295
00296 if( lEmergencyStop < 0 )
00297 {
00298 ROS_ERROR("\n\nAllegro Hand Node is Shutting Down! (Emergency Stop)\n");
00299 ros::shutdown();
00300 }
00301
00302
00303
00304
00305
00306
00307 for(int i=0; i<DOF_JOINTS; i++)
00308 {
00309 current_position_filtered[i] = (0.6*current_position_filtered[i]) + (0.198*previous_position[i]) + (0.198*current_position[i]);
00310 current_velocity[i] = (current_position_filtered[i] - previous_position_filtered[i]) / dt;
00311 current_velocity_filtered[i] = (0.6*current_velocity_filtered[i]) + (0.198*previous_velocity[i]) + (0.198*current_velocity[i]);
00312 }
00313
00314
00315
00316
00317 if(frame>100)
00318 {
00319 for(int i=0; i<DOF_JOINTS; i++)
00320 {
00321 desired_torque[i] = k_p[i]*(desired_position[i]-current_position_filtered[i]) - k_d[i]*current_velocity_filtered[i];
00322 desired_torque[i] = desired_torque[i]/800.0;
00323 }
00324 }
00325
00326
00327
00328
00329
00330
00331
00332
00333 msgJoint.header.stamp = tnow;
00334 for(int i=0; i<DOF_JOINTS; i++)
00335 {
00336 msgJoint.position[i] = current_position_filtered[i];
00337 msgJoint.velocity[i] = current_velocity_filtered[i];
00338 msgJoint.effort[i] = desired_torque[i];
00339 }
00340 joint_state_pub.publish(msgJoint);
00341
00342 frame++;
00343
00344
00345
00346
00347
00348
00349
00350 ros::spinOnce();
00351 rate.sleep();
00352 }
00353
00354
00355
00356
00357
00358 nh.shutdown();
00359 delete canDevice;
00360 printf("\nAllegro Hand Node has been shut down. Bye!\n\n");
00361 return 0;
00362
00363 }
00364
00365