iob.cpp
Go to the documentation of this file.
00001 #include <unistd.h>
00002 #include <iostream>
00003 #include <string>
00004 #include <cstdlib>
00005 #include <cstring>
00006 #include <vector>
00007 #include "io/iob.h"
00008 
00009 #include <ros/ros.h>
00010 #include <boost/algorithm/string.hpp>
00011 #include <osrf_msgs/JointCommands.h>
00012 #include <sensor_msgs/JointState.h>
00013 #include <sensor_msgs/Imu.h>
00014 #include <atlas_msgs/AtlasState.h>
00015 #include <atlas_msgs/AtlasCommand.h>
00016 #include <hrpUtil/Eigen3d.h>
00017 
00018 static ros::NodeHandle* rosnode;
00019 static ros::Publisher pub_joint_commands_;
00020 static ros::Subscriber sub_atlas_state;
00021 static atlas_msgs::AtlasCommand jointcommands;
00022 static atlas_msgs::AtlasCommand initial_jointcommands;
00023 //static osrf_msgs::JointCommands jointcommands;
00024 static atlas_msgs::AtlasState js;
00025 static int init_sub_flag = FALSE;
00026 
00027 static std::vector<double> command;
00028 static std::vector<double> prev_command;
00029 static std::vector<std::vector<double> > forces;
00030 static std::vector<std::vector<double> > gyros;
00031 static std::vector<std::vector<double> > accelerometers;
00032 static std::vector<std::vector<double> > attitude_sensors;
00033 static std::vector<std::vector<double> > force_offset;
00034 static std::vector<std::vector<double> > gyro_offset;
00035 static std::vector<std::vector<double> > accel_offset;
00036 static std::vector<int> power;
00037 static std::vector<int> servo;
00038 static bool isLocked = false;
00039 static int frame = 0;
00040 static timespec g_ts;
00041 static long g_period_ns=3000000;
00042 static ros::Time rg_ts;
00043 static ros::Time last_callback_time;
00044 
00045 #define CHECK_JOINT_ID(id) if ((id) < 0 || (id) >= number_of_joints()) return E_ID
00046 #define CHECK_FORCE_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_force_sensors()) return E_ID
00047 #define CHECK_ACCELEROMETER_ID(id) if ((id) < 0 || (id) >= number_of_accelerometers()) return E_ID
00048 #define CHECK_GYRO_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_gyro_sensors()) return E_ID
00049 #define CHECK_ATTITUDE_SENSOR_ID(id) if ((id) < 0 || (id) >= number_of_attitude_sensors()) return E_ID
00050 
00051 #if 0
00052 // atlas_v0
00053 Collada Robot Model (MODEL)
00054 // This order comes from order of <attachment_full joint joint="***"> tag in hrpsys_gazebo_atlas/models/atlas.dae
00055  [0] back_lbz
00056  [1] back_mby
00057  [2] back_ubx
00058  [3] l_arm_usy
00059  [4] l_arm_shx
00060  [5] l_arm_ely
00061  [6] l_arm_elx
00062  [7] l_arm_uwy
00063  [8] l_arm_mwx
00064  [9] neck_ay
00065 [10] center_bottom_led_frame_joint
00066 [11] center_top_led_frame_joint
00067 [12] head_imu_joint
00068 [13] hokuyo_joint
00069 [14] head_hokuyo_joint
00070 [15] left_camera_frame_joint
00071 [16] left_camera_optical_frame_joint
00072 [17] left_led_frame_joint
00073 [18] right_camera_frame_joint
00074 [19] right_camera_optical_frame_joint
00075 [20] right_led_frame_joint
00076 [21] r_arm_usy
00077 [22] r_arm_shx
00078 [23] r_arm_ely
00079 [24] r_arm_elx
00080 [25] r_arm_uwy
00081 [26] r_arm_mwx
00082 [27] imu_joint
00083 [28] l_leg_uhz
00084 [29] l_leg_mhx
00085 [30] l_leg_lhy
00086 [31] l_leg_kny
00087 [32] l_leg_uay
00088 [33] l_leg_lax
00089 [34] r_leg_uhz
00090 [35] r_leg_mhx
00091 [36] r_leg_lhy
00092 [37] r_leg_kny
00093 [38] r_leg_uay
00094 [39] r_leg_lax
00095 gazebo robot model (REAL)
00096 //This order comes from ros topic atlas/joint_states/name
00097 'back_lbz'
00098 'back_mby'
00099 'back_ubx'
00100 'neck_ay'
00101 'l_leg_uhz'
00102 'l_leg_mhx'
00103 'l_leg_lhy'
00104 'l_leg_kny'
00105 'l_leg_uay'
00106 'l_leg_lax'
00107 'r_leg_uhz'
00108 'r_leg_mhx'
00109 'r_leg_lhy'
00110 'r_leg_kny'
00111 'r_leg_uay'
00112 'r_leg_lax'
00113 'l_arm_usy'
00114 'l_arm_shx'
00115 'l_arm_ely'
00116 'l_arm_elx'
00117 'l_arm_uwy'
00118 'l_arm_mwx'
00119 'r_arm_usy'
00120 'r_arm_shx'
00121 'r_arm_ely'
00122 'r_arm_elx'
00123 'r_arm_uwy'
00124 'r_arm_mwx'
00125 
00126 // atals_v3
00127 Collada Robot Model (MODEL)
00128 // This order comes from order of <attachment_full joint joint="***"> tag in hrpsys_gazebo_atlas/models/atlas_v3.dae
00129  [0] back_bkz
00130  [1] back_bky
00131  [2] back_bkx
00132  [3] l_arm_shy
00133  [4] l_arm_shx
00134  [5] l_arm_ely
00135  [6] l_arm_elx
00136  [7] l_arm_wry
00137  [8] l_arm_wrx
00138  [9] l_situational_awareness_camera_joint
00139 [10] l_situational_awareness_camera_optical_frame_joint
00140 [11] neck_ry
00141 [12] center_bottom_led_frame_joint
00142 [13] center_top_led_frame_joint
00143 [14] head_imu_joint
00144 [15] hokuyo_joint
00145 [16] head_hokuyo_joint
00146 [17] left_camera_frame_joint
00147 [18] left_camera_optical_frame_joint
00148 [19] left_led_frame_joint
00149 [20] right_camera_frame_joint
00150 [21] right_camera_optical_frame_joint
00151 [22] right_led_frame_joint
00152 [23] r_arm_shy
00153 [24] r_arm_shx
00154 [25] r_arm_ely
00155 [26] r_arm_elx
00156 [27] r_arm_wry
00157 [28] r_arm_wrx
00158 [29] r_situational_awareness_camera_joint
00159 [30] r_situational_awareness_camera_optical_frame_joint
00160 [31] rear_situational_awareness_camera_joint
00161 [32] rear_situational_awareness_camera_optical_frame_joint
00162 [33] imu_joint
00163 [34] l_leg_hpz
00164 [35] l_leg_hpx
00165 [36] l_leg_hpy
00166 [37] l_leg_kny
00167 [38] l_leg_aky
00168 [39] l_leg_akx
00169 [40] r_leg_hpz
00170 [41] r_leg_hpx
00171 [42] r_leg_hpy
00172 [43] r_leg_kny
00173 [44] r_leg_aky
00174 [45] r_leg_akx
00175 
00176 gazebo robot model (REAL)
00177 //This order comes from ros topic atlas/joint_states/name
00178 'back_bkz'
00179 'back_bky'
00180 'back_bkx'
00181 'neck_ry'
00182 'l_leg_hpz'
00183 'l_leg_hpx'
00184 'l_leg_hpy'
00185 'l_leg_kny'
00186 'l_leg_aky'
00187 'l_leg_akx'
00188 'r_leg_hpz'
00189 'r_leg_hpx'
00190 'r_leg_hpy'
00191 'r_leg_kny'
00192 'r_leg_aky'
00193 'r_leg_akx'
00194 'l_arm_shy'
00195 'l_arm_shx'
00196 'l_arm_ely'
00197 'l_arm_elx'
00198 'l_arm_wry'
00199 'l_arm_wrx'
00200 'r_arm_shy'
00201 'r_arm_shx'
00202 'r_arm_ely'
00203 'r_arm_elx'
00204 'r_arm_wry' 
00205 'r_arm_wrx'
00206 #endif
00207 
00208 #define JOINT_ID_REAL2MODEL(id) get_joint_id_real2model(id)
00209 #define JOINT_ID_MODEL2REAL(id) get_joint_id_model2real(id)
00210 #define NUM_OF_REAL_JOINT get_num_of_real_joint()
00211 /* for atlas_v0 (old model without backpack) */
00212 static int joint_id_real2model_atlas_v0[] = {0, 1, 2, 9, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 3, 4, 5, 6, 7, 8, 21, 22, 23, 24, 25, 26};
00213 /* for atlas_v3 */
00214 static int joint_id_real2model_atlas_v3[] = {0, 1, 2, 11, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 3, 4, 5, 6, 7, 8, 23, 24, 25, 26, 27, 28};
00215 /* robot idx */
00216 enum AtlasName {ATLAS_V0, ATLAS_V3, UNKNOWN};
00217 static enum AtlasName atlas_name = UNKNOWN;
00218 
00219 static void update_atlas_name()
00220 {
00221   std::string iob_atlas_name_value;
00222   rosnode->getParam("iob_atlas_name", iob_atlas_name_value);
00223   if (iob_atlas_name_value == "atlas_v0") {
00224     ROS_WARN_STREAM("atlas_name is ATLAS_V0");
00225     atlas_name = ATLAS_V0;
00226   } else if (iob_atlas_name_value == "atlas_v3") {
00227     atlas_name = ATLAS_V3;
00228     ROS_WARN_STREAM("atlas_name is ATLAS_V3");
00229   } else {
00230     ROS_ERROR_STREAM("atlas_name is INVALID");
00231   }
00232 }
00233 
00234 static int get_joint_id_real2model(int id)
00235 {
00236   if (atlas_name == UNKNOWN) {
00237     update_atlas_name();
00238   }
00239 
00240   if (atlas_name == ATLAS_V0) {
00241     return joint_id_real2model_atlas_v0[id];
00242   } else if(atlas_name == ATLAS_V3) {
00243     return joint_id_real2model_atlas_v3[id];
00244   } else {
00245     return -1;
00246   }
00247 }
00248 static int get_num_of_real_joint()
00249 {
00250   if (atlas_name == UNKNOWN) {
00251     update_atlas_name();
00252   }
00253 
00254   if (atlas_name == ATLAS_V0) {
00255     return sizeof(joint_id_real2model_atlas_v0)/sizeof(joint_id_real2model_atlas_v0[0]);
00256   } else if(atlas_name == ATLAS_V3) {
00257     return sizeof(joint_id_real2model_atlas_v3)/sizeof(joint_id_real2model_atlas_v3[0]);
00258   } else {
00259     return -1;
00260   }
00261 }
00262 
00263 static int get_joint_id_model2real(int id)
00264 {
00265   for (int i = 0; i < NUM_OF_REAL_JOINT; i++){
00266     if (get_joint_id_real2model(i) == id){
00267       return i;
00268     }
00269   }
00270   return -1;
00271 }
00272 
00273 #ifdef __APPLE__
00274 typedef int clockid_t;
00275 #define CLOCK_MONOTONIC 0
00276 #include <mach/mach_time.h>
00277 int clock_gettime(clockid_t clk_id, struct timespec *tp)
00278 {
00279     if (clk_id != CLOCK_MONOTONIC) return -1;
00280 
00281     uint64_t clk;
00282     clk = mach_absolute_time();
00283 
00284     static mach_timebase_info_data_t info = {0,0};
00285     if (info.denom == 0) mach_timebase_info(&info);
00286 
00287     uint64_t elapsednano = clk * (info.numer / info.denom);
00288 
00289     tp->tv_sec = elapsednano * 1e-9;
00290     tp->tv_nsec = elapsednano - (tp->tv_sec * 1e9);
00291     return 0;
00292 }
00293 
00294 #define TIMER_ABSTIME 0
00295 int clock_nanosleep(clockid_t clk_id, int flags, struct timespec *tp,
00296     struct timespec *remain)
00297 {
00298     if (clk_id != CLOCK_MONOTONIC || flags != TIMER_ABSTIME) return -1;
00299 
00300     static mach_timebase_info_data_t info = {0,0};
00301     if (info.denom == 0) mach_timebase_info(&info);
00302 
00303     uint64_t clk = (tp->tv_sec*1e9 + tp->tv_nsec)/(info.numer/info.denom);
00304 
00305     mach_wait_until(clk);
00306     return 0;
00307 }
00308 #endif
00309 
00310 
00311 int number_of_joints()
00312 {
00313     return (int)command.size();
00314 }
00315 
00316 int number_of_force_sensors()
00317 {
00318     return (int)forces.size();
00319 }
00320 
00321 int number_of_gyro_sensors()
00322 {
00323     return (int)gyros.size();
00324 }
00325 
00326 int number_of_accelerometers()
00327 {
00328     return (int)accelerometers.size();
00329 }
00330 
00331 int number_of_attitude_sensors()
00332 {
00333     return (int)attitude_sensors.size();
00334 }
00335 
00336 int set_number_of_joints(int num)
00337 {
00338     command.resize(num);
00339     prev_command.resize(num);
00340     power.resize(num);
00341     servo.resize(num);
00342     for (int i=0; i<num; i++){
00343         command[i] = power[i] = servo[i] = prev_command[i] = 0;
00344     }
00345     return TRUE;
00346 }
00347 
00348 int set_number_of_force_sensors(int num)
00349 {
00350     forces.resize(num);
00351     force_offset.resize(num);
00352     for (unsigned int i=0; i<forces.size();i++){
00353         forces[i].resize(6);
00354         force_offset[i].resize(6);
00355         for (int j=0; j<6; j++){
00356             forces[i][j] = 0;
00357             force_offset[i][j] = 0;
00358         }
00359     }
00360     return TRUE;
00361 }
00362 
00363 int set_number_of_gyro_sensors(int num)
00364 {
00365     gyros.resize(num);
00366     gyro_offset.resize(num);
00367     for (unsigned int i=0; i<gyros.size();i++){
00368         gyros[i].resize(3);
00369         gyro_offset[i].resize(3);
00370         for (int j=0; j<3; j++){
00371             gyros[i][j] = 0.0;
00372             gyro_offset[i][j] = 0.0;
00373         }
00374     }
00375     return TRUE;
00376 }
00377 
00378 int set_number_of_accelerometers(int num)
00379 {
00380     accelerometers.resize(num);
00381     accel_offset.resize(num);
00382     for (unsigned int i=0; i<accelerometers.size();i++){
00383         accelerometers[i].resize(3);
00384         accel_offset[i].resize(3);
00385         for (int j=0; j<3; j++){
00386             accelerometers[i][j] = j == 2 ? 9.81 : 0.0;
00387             accel_offset[i][j] = 0;
00388         }
00389     }
00390     return TRUE;
00391 }
00392 
00393 int set_number_of_attitude_sensors(int num)
00394 {
00395     attitude_sensors.resize(num);
00396     for (unsigned int i=0; i<attitude_sensors.size();i++){
00397         attitude_sensors[i].resize(3);
00398         for (int j=0; j<3; j++){
00399             attitude_sensors[i][j] = 0.0;
00400         }
00401     }
00402     return TRUE;
00403 }
00404 
00405 int read_power_state(int id, int *s)
00406 {
00407     CHECK_JOINT_ID(id);
00408     *s = power[id];
00409     return TRUE;
00410 }
00411 
00412 int write_power_command(int id, int com)
00413 {
00414     CHECK_JOINT_ID(id);
00415     power[id] = com;
00416     return TRUE;
00417 }
00418 
00419 int read_power_command(int id, int *com)
00420 {
00421     CHECK_JOINT_ID(id);
00422     *com = power[id];
00423     return TRUE;
00424 }
00425 
00426 int read_servo_state(int id, int *s)
00427 {
00428     CHECK_JOINT_ID(id);
00429     *s = servo[id];
00430     return TRUE;
00431 }
00432 
00433 int read_servo_alarm(int id, int *a)
00434 {
00435     CHECK_JOINT_ID(id);
00436     *a = 0;
00437     return TRUE;
00438 }
00439 
00440 int read_control_mode(int id, joint_control_mode *s)
00441 {
00442     CHECK_JOINT_ID(id);
00443     *s = JCM_POSITION;
00444     return TRUE;
00445 }
00446 
00447 int write_control_mode(int id, joint_control_mode s)
00448 {
00449     CHECK_JOINT_ID(id);
00450     return TRUE;
00451 }
00452 
00453 int read_actual_angle(int id, double *angle)
00454 {
00455   CHECK_JOINT_ID(id);
00456   //*angle = command[id]+0.01;
00457   //*angle = command[id];
00458   if(init_sub_flag){
00459     if (JOINT_ID_MODEL2REAL(id) < 0) {
00460       *angle = command[id];
00461     }else{
00462       *angle = js.position[JOINT_ID_MODEL2REAL(id)];
00463     }
00464   }else{
00465     *angle = command[id];
00466   }
00467   return TRUE;
00468 }
00469 
00470 int read_actual_angles(double *angles)
00471 {
00472     for (int i=0; i<number_of_joints(); i++){
00473         read_actual_angle(i, &angles[i]);
00474     }
00475     return TRUE;
00476 }
00477 
00478 int read_actual_torques(double *torques)
00479 {
00480   // return FALSE;
00481 
00482   if(init_sub_flag) {
00483     for(int i=0; i<number_of_joints(); i++){
00484       if(JOINT_ID_MODEL2REAL(i) < 0) {
00485         *(torques+i) = -1;
00486       }else{
00487         *(torques+i) = js.effort[JOINT_ID_MODEL2REAL(i)];
00488       }
00489     }
00490   }
00491 
00492   return TRUE;
00493 }
00494 
00495 int read_command_torque(int id, double *torque)
00496 {
00497     return FALSE;
00498 }
00499 
00500 int write_command_torque(int id, double torque)
00501 {
00502     return FALSE;
00503 }
00504 
00505 int read_command_torques(double *torques)
00506 {
00507     return FALSE;
00508 }
00509 
00510 int write_command_torques(const double *torques)
00511 {
00512     return FALSE;
00513 }
00514 
00515 int read_command_angle(int id, double *angle)
00516 {
00517     CHECK_JOINT_ID(id);
00518     *angle = command[id];
00519     return TRUE;
00520 }
00521 
00522 int write_command_angle(int id, double angle)
00523 {
00524     CHECK_JOINT_ID(id);
00525     command[id] = angle;
00526     return TRUE;
00527 }
00528 
00529 int read_command_angles(double *angles)
00530 {
00531     for (int i=0; i<number_of_joints(); i++){
00532         angles[i] = command[i];
00533     }
00534     return TRUE;
00535 }
00536 
00537 int write_command_angles(const double *angles)
00538 {
00539     for (int i=0; i<number_of_joints(); i++){
00540         prev_command[i] = command[i];
00541         command[i] = angles[i];
00542     }
00543 
00544     atlas_msgs::AtlasCommand send_com(jointcommands);
00545     bool servo_on = true;
00546 
00547     send_com.header.stamp = ros::Time::now();
00548 
00549     for (int i=0; i<NUM_OF_REAL_JOINT; i++) {
00550       if (servo[JOINT_ID_REAL2MODEL(i)] > 0) {
00551         send_com.position[i] = command[JOINT_ID_REAL2MODEL(i)];
00552         send_com.velocity[i] = (command[JOINT_ID_REAL2MODEL(i)] - prev_command[JOINT_ID_REAL2MODEL(i)]) / (g_period_ns * 1e-9);
00553         if(atlas_name == ATLAS_V3) {
00554           if(i == 8 || i == 9 || i == 14 || i == 15) {
00555             send_com.kp_velocity[i]  = 0;
00556           } else {
00557             send_com.kp_velocity[i]  = 50;
00558           }
00559         } else if(atlas_name == ATLAS_V0) {
00560           send_com.kp_velocity[i]  = 200;
00561         }
00562       } else {
00563         servo_on = false;
00564         send_com.position[i] = 0;
00565         send_com.velocity[i] = 0;
00566         send_com.effort[i] = 0;
00567         send_com.kp_position[i] = 0;
00568         send_com.ki_position[i] = 0;
00569         send_com.kd_position[i] = 0;
00570         send_com.kp_velocity[i]  = 0;
00571         send_com.kp_velocity[i]  = 0;
00572       }
00573     }
00574 
00575     if (servo_on) {
00576       // ROS_INFO_STREAM("pub: " << send_com);
00577       pub_joint_commands_.publish(send_com);
00578     }
00579 
00580     // check if the joint_states are published
00581     if ( ros::Time::now()  - last_callback_time > ros::Duration(10.0) ) {
00582       ROS_ERROR_STREAM("/atlas/atlas_state  has not been published last 10.0 seconds, check by `rostopic info /atlas/atlas_state`");
00583       last_callback_time = ros::Time::now();
00584     }
00585 
00586     ros::spinOnce();
00587 
00588     return TRUE;
00589 }
00590 
00591 int read_pgain(int id, double *gain)
00592 {
00593   if(JOINT_ID_MODEL2REAL(id) < 0) {
00594     //
00595     std::cerr << ";;; read gain: " << id << " failed." << std::endl;
00596   }else{
00597     int iid = JOINT_ID_MODEL2REAL(id);
00598     *(gain) = jointcommands.kp_position[iid] / initial_jointcommands.kp_position[iid];
00599     //std::cerr << ";;; read gain: " << id << " = " << *gain << std::endl;
00600   }
00601   return TRUE;
00602 }
00603 
00604 int write_pgain(int id, double gain)
00605 {
00606   //std::cerr << ";;; write pgain: " << id << " = " << gain << std::endl;
00607   if(JOINT_ID_MODEL2REAL(id) < 0) {
00608     //
00609   }else{
00610     int iid = JOINT_ID_MODEL2REAL(id);
00611     jointcommands.kp_position[iid] =
00612       gain * initial_jointcommands.kp_position[iid];
00613   }
00614   return TRUE;
00615 }
00616 
00617 int read_dgain(int id, double *gain)
00618 {
00619   //std::cerr << ";;; read dgain: " << id << " = " << *gain << std::endl;
00620   if(JOINT_ID_MODEL2REAL(id) < 0) {
00621     //
00622   }else{
00623     int iid = JOINT_ID_MODEL2REAL(id);
00624     *(gain) = jointcommands.kd_position[iid] / initial_jointcommands.kd_position[iid];
00625   }
00626   return TRUE;
00627 }
00628 
00629 int write_dgain(int id, double gain)
00630 {
00631   //std::cerr << ";;; write dgain: " << id << " = " << gain << std::endl;
00632   if(JOINT_ID_MODEL2REAL(id) < 0) {
00633     //
00634   }else{
00635     int iid = JOINT_ID_MODEL2REAL(id);
00636     jointcommands.kd_position[iid] =
00637       gain * initial_jointcommands.kd_position[iid];
00638   }
00639   return TRUE;
00640 }
00641 
00642 int read_force_sensor(int id, double *forces)
00643 {
00644   CHECK_FORCE_SENSOR_ID(id);
00645   // for (int i=0; i<6; i++){
00646   //     forces[i] = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*2
00647   //         + 2 + force_offset[id][i]; // 2 = initial offset
00648   // }
00649   std::vector<geometry_msgs::Wrench*> fsensors;
00650   fsensors.push_back(&(js.l_hand));
00651   fsensors.push_back(&(js.r_hand));
00652   fsensors.push_back(&(js.l_foot));
00653   fsensors.push_back(&(js.r_foot));
00654   if(init_sub_flag){
00655     forces[0] = fsensors[id]->force.x + force_offset[id][0];
00656     forces[1] = fsensors[id]->force.y + force_offset[id][1];
00657     forces[2] = fsensors[id]->force.z + force_offset[id][2];
00658     forces[3] = fsensors[id]->torque.x + force_offset[id][3];
00659     forces[4] = fsensors[id]->torque.y + force_offset[id][4];
00660     forces[5] = fsensors[id]->torque.z + force_offset[id][5];
00661   }
00662 
00663   return TRUE;
00664 }
00665 
00666 int read_gyro_sensor(int id, double *rates)
00667 {
00668   CHECK_GYRO_SENSOR_ID(id);
00669   // for (int i=0; i<3; i++){
00670   //     rates[i] = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.01
00671   //         + 0.01 + gyro_offset[id][i]; // 0.01 = initial offset
00672   // }
00673   if(init_sub_flag){
00674     
00675     // use atlas orientation
00676     Eigen::Quaternion<double> q(js.orientation.w,
00677                                 js.orientation.x,
00678                                 js.orientation.y,
00679                                 js.orientation.z);
00680     hrp::Vector3 rpy = hrp::rpyFromRot(q.toRotationMatrix());
00681     rates[0] = rpy[0];
00682     rates[1] = rpy[1];
00683     rates[2] = rpy[2];
00684 
00685     // rates[0] = js.angular_velocity.x
00686     //   + 0.01 + gyro_offset[id][0]; // 0.01 = initial offset
00687     // rates[1] = js.angular_velocity.y
00688     //   + 0.01 + gyro_offset[id][1]; // 0.01 = initial offset
00689     // rates[2] = js.angular_velocity.z
00690     //   + 0.01 + gyro_offset[id][2]; // 0.01 = initial offset
00691   } else {
00692     // tempolary values when sensor is not ready.
00693     rates[0] = rates[1] = rates[2] = 0.0;
00694   }
00695 
00696   return TRUE;
00697 }
00698 
00699 int read_accelerometer(int id, double *accels)
00700 {
00701   CHECK_ACCELEROMETER_ID(id);
00702   // for (int i=0; i<3; i++){
00703   //     double randv = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.01;
00704   //     accels[i] = (i == 2 ? (9.8+randv) : randv)
00705   //         + 0.01 + accel_offset[id][i]; // 0.01 = initial offset
00706   // }
00707   if(init_sub_flag){
00708     accels[0] = js.linear_acceleration.x
00709       + 0.01 + accel_offset[id][0]; // 0.01 = initial offset
00710     accels[1] = js.linear_acceleration.y
00711       + 0.01 + accel_offset[id][1]; // 0.01 = initial offset
00712     accels[2] = js.linear_acceleration.z
00713       + 0.01 + accel_offset[id][2]; // 0.01 = initial offset
00714   } else {
00715     // tempolary values when sensor is not ready.
00716     accels[0] = accels[1] = 0.0;
00717     accels[2] = 9.8;
00718   }
00719 
00720   return TRUE;
00721 }
00722 
00723 int read_touch_sensors(unsigned short *onoff)
00724 {
00725     return FALSE;
00726 }
00727 
00728 int read_attitude_sensor(int id, double *att)
00729 {
00730   return FALSE;
00731 }
00732 
00733 int read_current(int id, double *mcurrent)
00734 {
00735     return FALSE;
00736 }
00737 
00738 int read_current_limit(int id, double *v)
00739 {
00740     return FALSE;
00741 }
00742 
00743 int read_currents(double *currents)
00744 {
00745     return FALSE;
00746 }
00747 
00748 int read_gauges(double *gauges)
00749 {
00750     return FALSE;
00751 }
00752 
00753 int read_actual_velocity(int id, double *vel)
00754 {
00755     return FALSE;
00756 }
00757 
00758 int read_command_velocity(int id, double *vel)
00759 {
00760     return FALSE;
00761 }
00762 
00763 int write_command_velocity(int id, double vel)
00764 {
00765     return FALSE;
00766 }
00767 
00768 int read_actual_velocities(double *vels)
00769 {
00770     return FALSE;
00771 }
00772 
00773 int read_command_velocities(double *vels)
00774 {
00775     return FALSE;
00776 }
00777 
00778 int write_command_velocities(const double *vels)
00779 {
00780     return FALSE;
00781 }
00782 
00783 int read_temperature(int id, double *v)
00784 {
00785     return FALSE;
00786 }
00787 
00788 int write_servo(int id, int com)
00789 {
00790     servo[id] = com;
00791     return TRUE;
00792 }
00793 
00794 int write_dio(unsigned short buf)
00795 {
00796     return FALSE;
00797 }
00798 
00799 // callback
00800 static void setJointStates(const atlas_msgs::AtlasState::ConstPtr &_js) {
00801   ROS_DEBUG(";; subscribe JointState");
00802   last_callback_time = ros::Time::now();
00803   js = *_js;
00804   init_sub_flag = TRUE;
00805 }
00806 
00807 int open_iob(void)
00808 {
00809     static bool isInitialized = false;
00810     if ( isInitialized ) return TRUE;
00811 
00812     std::map<std::string, std::string> arg;
00813     ros::init(arg, "hrpsys_gazebo_atlas", ros::init_options::NoSigintHandler);
00814     rosnode = new ros::NodeHandle();
00815 
00816     // http://gazebosim.org/wiki/Tutorials/drcsim/2.2/sending_joint_controller_commands_over_ros
00817     // Hardcoded List of Joint Names
00818     // List of joint names in the Atlas robot. Note the order must not change for this function to work correctly.
00819     if (atlas_name == UNKNOWN) {
00820       update_atlas_name();
00821     }
00822 
00823     std::vector<std::string> names;
00824     if (atlas_name == ATLAS_V0) {
00825     /* for atlas_v0 (old model without backpack) */
00826     names.push_back("atlas::back_lbz"); // 0
00827     names.push_back("atlas::back_mby"); // 1
00828     names.push_back("atlas::back_ubx"); // 2
00829     names.push_back("atlas::neck_ay"); // 9
00830     names.push_back("atlas::l_leg_uhz"); // 28
00831     names.push_back("atlas::l_leg_mhx"); // 29
00832     names.push_back("atlas::l_leg_lhy"); // 30
00833     names.push_back("atlas::l_leg_kny"); // 31
00834     names.push_back("atlas::l_leg_uay"); // 32
00835     names.push_back("atlas::l_leg_lax"); // 33
00836     names.push_back("atlas::r_leg_uhz"); // 34
00837     names.push_back("atlas::r_leg_mhx"); // 35
00838     names.push_back("atlas::r_leg_lhy"); // 36
00839     names.push_back("atlas::r_leg_kny"); // 37
00840     names.push_back("atlas::r_leg_uay"); // 38
00841     names.push_back("atlas::r_leg_lax"); // 39
00842     names.push_back("atlas::l_arm_usy"); // 3
00843     names.push_back("atlas::l_arm_shx"); // 4
00844     names.push_back("atlas::l_arm_ely"); // 5
00845     names.push_back("atlas::l_arm_elx"); // 6
00846     names.push_back("atlas::l_arm_uwy"); // 7
00847     names.push_back("atlas::l_arm_mwx"); // 8
00848     names.push_back("atlas::r_arm_usy"); // 21
00849     names.push_back("atlas::r_arm_shx"); // 22
00850     names.push_back("atlas::r_arm_ely"); // 23
00851     names.push_back("atlas::r_arm_elx"); // 24
00852     names.push_back("atlas::r_arm_uwy"); // 25
00853     names.push_back("atlas::r_arm_mwx"); // 26
00854     } else if (atlas_name == ATLAS_V3) {
00855       /* for atlas_v3 */
00856       names.push_back("atlas::back_bkz"); // 0
00857       names.push_back("atlas::back_bky"); // 1
00858       names.push_back("atlas::back_bkx"); // 2
00859       names.push_back("atlas::neck_ry"); // 9
00860       names.push_back("atlas::l_leg_hpz"); // 28
00861       names.push_back("atlas::l_leg_hpx"); // 29
00862       names.push_back("atlas::l_leg_hpy"); // 30
00863       names.push_back("atlas::l_leg_kny"); // 31
00864       names.push_back("atlas::l_leg_aky"); // 32
00865       names.push_back("atlas::l_leg_akx"); // 33
00866       names.push_back("atlas::r_leg_hpz"); // 34
00867       names.push_back("atlas::r_leg_hpx"); // 35
00868       names.push_back("atlas::r_leg_hpy"); // 36
00869       names.push_back("atlas::r_leg_kny"); // 37
00870       names.push_back("atlas::r_leg_aky"); // 38
00871       names.push_back("atlas::r_leg_akx"); // 39
00872       names.push_back("atlas::l_arm_shy"); // 3
00873       names.push_back("atlas::l_arm_shx"); // 4
00874       names.push_back("atlas::l_arm_ely"); // 5
00875       names.push_back("atlas::l_arm_elx"); // 6
00876       names.push_back("atlas::l_arm_wry"); // 7
00877       names.push_back("atlas::l_arm_wrx"); // 8
00878       names.push_back("atlas::r_arm_shy"); // 21
00879       names.push_back("atlas::r_arm_shx"); // 22
00880       names.push_back("atlas::r_arm_ely"); // 23
00881       names.push_back("atlas::r_arm_elx"); // 24
00882       names.push_back("atlas::r_arm_wry"); // 25
00883       names.push_back("atlas::r_arm_wrx"); // 26
00884     } else {
00885       ROS_ERROR_STREAM("atlas_name is INVALID");
00886     }
00887     unsigned int n = names.size();
00888 
00889     jointcommands.position.resize(n);
00890     jointcommands.velocity.resize(n);
00891     jointcommands.effort.resize(n);
00892     jointcommands.kp_position.resize(n);
00893     jointcommands.ki_position.resize(n);
00894     jointcommands.kd_position.resize(n);
00895     jointcommands.kp_velocity.resize(n);
00896     jointcommands.i_effort_min.resize(n);
00897     jointcommands.i_effort_max.resize(n);
00898 
00899     for (unsigned int i = 0; i < n; i++) {
00900         std::vector<std::string> pieces;
00901         boost::split(pieces, names[i], boost::is_any_of(":"));
00902         double kp;
00903         rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/p", kp);
00904         jointcommands.kp_position[i] = kp;
00905 
00906         double ki;
00907         rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i", ki);
00908         jointcommands.ki_position[i] = ki;
00909 
00910         double kd;
00911         rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/d", kd);
00912         jointcommands.kd_position[i] = kd;
00913 
00914         double emin;
00915         rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp", emin);
00916         jointcommands.i_effort_min[i] = -emin;
00917 
00918         double emax;
00919         rosnode->getParam("atlas_controller/gains/" + pieces[2] + "/i_clamp", emax);
00920         jointcommands.i_effort_max[i] = emax;
00921 
00922         jointcommands.velocity[i]     = 0;
00923         jointcommands.effort[i]       = 0;
00924         // jointcommands.kp_velocity[i]  = 50;
00925         if(atlas_name == ATLAS_V3) {
00926           if(i == 8 || i == 9 || i == 14 || i == 15) {
00927             jointcommands.kp_velocity[i]  = 0;
00928           } else {
00929             jointcommands.kp_velocity[i]  = 50;
00930           }
00931         } else if(atlas_name == ATLAS_V0) {
00932           jointcommands.kp_velocity[i]  = 200;
00933         }
00934     }
00935     jointcommands.desired_controller_period_ms = static_cast<unsigned int>(g_period_ns * 1e-6);
00936     //
00937     char *ret = getenv("HRPSYS_GAZEBO_ATLAS_COMMAND_TOPIC_NAME");
00938     std::string command_topic_name;
00939     if (ret != NULL) {
00940         command_topic_name.assign(ret);
00941     } else {
00942         command_topic_name = "/atlas/atlas_command";
00943     }
00944     ROS_INFO_STREAM( "[iob] HRPSYS_GAZEBO_ATLAS_COMMAND_TOPIC_NAME : " << command_topic_name);
00945     // pub_joint_commands_ = rosnode->advertise<osrf_msgs::JointCommands>("/atlas/joint_commands", 1, true);
00946     // pub_joint_commands_ = rosnode->advertise <atlas_msgs::AtlasCommand> ("/atlas/atlas_command", 1, true);
00947     pub_joint_commands_ = rosnode->advertise <atlas_msgs::AtlasCommand> (command_topic_name, 1, true);
00948 
00949     // subscribe
00950     // ros topic subscribtions
00951     last_callback_time = ros::Time::now();
00952     ros::SubscribeOptions jointStatesSo =
00953       ros::SubscribeOptions::create<atlas_msgs::AtlasState>("/atlas/atlas_state", 1, setJointStates,
00954                                                             ros::VoidPtr(), rosnode->getCallbackQueue());
00955     // Because TCP causes bursty communication with high jitter,
00956     // declare a preference on UDP connections for receiving
00957     // joint states, which we want to get at a high rate.
00958     // Note that we'll still accept TCP connections for this topic
00959     // (e.g., from rospy nodes, which don't support UDP);
00960     // we just prefer UDP.
00961     jointStatesSo.transport_hints =
00962       ros::TransportHints().unreliable().reliable().tcpNoDelay(true);
00963 
00964     sub_atlas_state = rosnode->subscribe(jointStatesSo);
00965 
00966 
00967     std::cout << "JointState IOB is opened" << std::endl;
00968     for (int i=0; i<number_of_joints(); i++){
00969         command[i] = 0.0;
00970         power[i] = OFF;
00971         servo[i] = OFF;
00972     }
00973     clock_gettime(CLOCK_MONOTONIC, &g_ts);
00974     rg_ts = ros::Time::now();
00975 
00976     initial_jointcommands = jointcommands;
00977     isInitialized = true;
00978     return TRUE;
00979 }
00980 
00981 int close_iob(void)
00982 {
00983     std::cout << "JointState IOB is closed" << std::endl;
00984     return TRUE;
00985 }
00986 
00987 int reset_body(void)
00988 {
00989     for (int i=0; i<number_of_joints(); i++){
00990         power[i] = servo[i] = OFF;
00991     }
00992     return TRUE;
00993 }
00994 
00995 int joint_calibration(int id, double angle)
00996 {
00997     return FALSE;
00998 }
00999 
01000 int read_gyro_sensor_offset(int id, double *offset)
01001 {
01002     for (int i=0; i<3; i++){
01003         offset[i] = gyro_offset[id][i];
01004     }
01005     return TRUE;
01006 }
01007 
01008 int write_gyro_sensor_offset(int id, double *offset)
01009 {
01010     for (int i=0; i<3; i++){
01011         gyro_offset[id][i] = offset[i];
01012     }
01013     return TRUE;
01014 }
01015 
01016 int read_accelerometer_offset(int id, double *offset)
01017 {
01018     for (int i=0; i<3; i++){
01019         offset[i] = accel_offset[id][i];
01020     }
01021     return TRUE;
01022 }
01023 
01024 int write_accelerometer_offset(int id, double *offset)
01025 {
01026     for (int i=0; i<3; i++){
01027         accel_offset[id][i] = offset[i];
01028     }
01029     return TRUE;
01030 }
01031 
01032 int read_force_offset(int id, double *offsets)
01033 {
01034     for (int i=0; i<6; i++){
01035         offsets[i] = force_offset[id][i];
01036     }
01037     return TRUE;
01038 }
01039 
01040 int write_force_offset(int id, double *offsets)
01041 {
01042     for (int i=0; i<6; i++){
01043         force_offset[id][i] = offsets[i];
01044     }
01045     return TRUE;
01046 }
01047 
01048 int write_attitude_sensor_offset(int id, double *offset)
01049 {
01050     return FALSE;
01051 }
01052 
01053 int read_calib_state(int id, int *s)
01054 {
01055     CHECK_JOINT_ID(id);
01056     int v = id/2;
01057     *s = v%2==0 ? ON : OFF;
01058     return TRUE;
01059 }
01060 
01061 int lock_iob()
01062 {
01063     if (isLocked) return FALSE;
01064 
01065     //isLocked = true;
01066     return TRUE;
01067 }
01068 int unlock_iob()
01069 {
01070     isLocked = false;
01071     return TRUE;
01072 }
01073 
01074 int read_lock_owner(pid_t *pid)
01075 {
01076   return FALSE;
01077 }
01078 
01079 int read_limit_angle(int id, double *angle)
01080 {
01081   return FALSE;
01082 }
01083 
01084 int read_angle_offset(int id, double *angle)
01085 {
01086   return FALSE;
01087 }
01088 
01089 int write_angle_offset(int id, double angle)
01090 {
01091   return FALSE;
01092 }
01093 
01094 int read_ulimit_angle(int id, double *angle)
01095 {
01096   return FALSE;
01097 }
01098 int read_llimit_angle(int id, double *angle)
01099 {
01100   return FALSE;
01101 }
01102 int read_encoder_pulse(int id, double *ec)
01103 {
01104   return FALSE;
01105 }
01106 int read_gear_ratio(int id, double *gr)
01107 {
01108   return FALSE;
01109 }
01110 int read_torque_const(int id, double *tc)
01111 {
01112   return FALSE;
01113 }
01114 int read_torque_limit(int id, double *limit)
01115 {
01116   return FALSE;
01117 }
01118 
01119 unsigned long long read_iob_frame()
01120 {
01121     ++frame;
01122     //if (frame == 5) frame = 0;
01123     return frame;
01124 }
01125 
01126 int number_of_substeps()
01127 {
01128     return 1;
01129 }
01130 
01131 int read_power(double *voltage, double *current)
01132 {
01133     *voltage = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*1+48;
01134     *current = ((double)random()-RAND_MAX/2)/(RAND_MAX/2)*0.5+1;
01135     return TRUE;
01136 }
01137 
01138 int read_driver_temperature(int id, unsigned char *v)
01139 {
01140     *v = id * 2;
01141     return TRUE;
01142 }
01143 
01144 void timespec_add_ns(timespec *ts, long ns)
01145 {
01146     ts->tv_nsec += ns;
01147     while (ts->tv_nsec > 1e9){
01148         ts->tv_sec += 1;
01149         ts->tv_nsec -= 1e9;
01150     }
01151 }
01152 
01153 double timespec_compare(timespec *ts1, timespec *ts2)
01154 {
01155     double dts = ts1->tv_sec - ts2->tv_sec;
01156     double dtn = ts1->tv_nsec - ts2->tv_nsec;
01157     return dts*1e9+dtn;
01158 }
01159 
01160 int wait_for_iob_signal()
01161 {
01162 #if 0
01163     // use system clock
01164     clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &g_ts, 0);
01165     timespec_add_ns(&g_ts, g_period_ns);
01166     timespec now;
01167     clock_gettime(CLOCK_MONOTONIC, &now);
01168     double dt = timespec_compare(&g_ts, &now);
01169 
01170     if (dt <= 0){
01171         fprintf(stderr, "iob::overrun (%d[ms])\n", -dt*1e6);
01172         do {
01173             timespec_add_ns(&g_ts, g_period_ns);
01174         }while(timespec_compare(&g_ts, &now)<=0);
01175     }
01176 #endif
01177 #if 1
01178     // use ROS Time
01179     //ros::Time::sleepUntil(rg_ts);
01180     // while (ros::Time::now() < rg_ts);
01181 
01182     // {
01183     //   ros::Duration tm = ros::Duration(0, g_period_ns);
01184     //   rg_ts += tm;
01185     // }
01186     // ros::Time rnow = ros::Time::now();
01187     // ros::Duration rdt = rg_ts - rnow;
01188 
01189     //ros::Time::sleepUntil(rg_ts);
01190     ros::Time rnow;
01191     ros::Duration tm = ros::Duration(0, g_period_ns);
01192     ros::WallDuration wtm = ros::WallDuration(0, 100000); // 0.1 ms
01193     while ((rnow = ros::Time::now()) < rg_ts) {
01194       wtm.sleep();
01195     }
01196 
01197     rg_ts += tm;
01198     if ((rg_ts - rnow).toSec() <= 0) {
01199       fprintf(stderr, "iob::overrun (%f[ms]), w:%f -> %f\n",
01200               (rnow - rg_ts).toSec()*1000, rnow.toSec(), rg_ts.toSec());
01201       do {
01202         rg_ts += tm;
01203       } while ((rg_ts - rnow).toSec() <= 0);
01204     }
01205 #endif
01206     return 0;
01207 }
01208 
01209 size_t length_of_extra_servo_state(int id)
01210 {
01211     return 0;
01212 }
01213 
01214 int read_extra_servo_state(int id, int *state)
01215 {
01216     return TRUE;
01217 }
01218 
01219 int set_signal_period(long period_ns)
01220 {
01221     g_period_ns = period_ns;
01222     return TRUE;
01223 }
01224 
01225 long get_signal_period()
01226 {
01227     return g_period_ns;
01228 }
01229 
01230 int initializeJointAngle(const char *name, const char *option)
01231 {
01232     sleep(3);
01233     return TRUE;
01234 }
01235 
01236 int read_digital_input(char *dinput)
01237 {
01238   return 0;
01239 }
01240 
01241 int write_digital_output(const char *doutput)
01242 {
01243   return 0;
01244 }
01245 
01246 int length_digital_input(void)
01247 {
01248   return 0;
01249 }
01250 
01251 int write_digital_output_with_mask(const char *doutput, const char *mask)
01252 {
01253   return FALSE;
01254 }
01255 
01256 int length_digital_output(void)
01257 {
01258   return 0;
01259 }
01260 
01261 int read_digital_output(char *doutput)
01262 {
01263   return 0;
01264 }
01265 


hrpsys_gazebo_atlas
Author(s): Yohei Kakiuchi
autogenerated on Wed Sep 16 2015 10:53:59