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


hrpsys_gazebo
Author(s): Yohei Kakiuchi
autogenerated on Thu Jun 27 2013 14:58:14