00001
00002
00003 code = """
00004 #include <stdio.h>
00005 #include <sys/types.h>
00006 #include <iob.h>
00007 int main() {
00008 int ret1, ret2;
00009 ret1 = open_iob();
00010 printf("open_iob returns %d\\n", ret1);
00011
00012 ret2 = close_iob();
00013 printf("close_iob returns %d\\n", ret2);
00014
00015 if ( ret1 && ret2 ) {
00016 return 0; // success
00017 } else {
00018 return 1; // failure
00019 }
00020 }
00021 """
00022
00023 import tempfile, os, subprocess, re, sys
00024 import shutil
00025 import binascii
00026
00027 def create_check_hrpiob_iob():
00028 ret = True
00029 d_tmp = tempfile.mkdtemp()
00030
00031 f_iob = open(os.path.join(d_tmp,"iob.h"),'w')
00032 f_iob.write(iob_h)
00033 f_iob.close()
00034
00035 f_code_i, f_code_name = tempfile.mkstemp(suffix='.cpp',dir=d_tmp)
00036 f_code = os.fdopen(f_code_i,'w')
00037 f_code.write(code)
00038 f_code.close()
00039
00040 print re.sub('(\n)', '\n - ', code)
00041 exename = os.path.splitext(f_code_name)[0]
00042 command = 'gcc -v -o %s %s -I%s -L/usr/pkg/lib -L/opt/nextage-open/lib -lhrpIo -lboost_thread -lboost_signals -lboost_filesystem -lboost_system -lboost_regex -lf2c -Wl,-rpath /usr/pkg/lib -Wl,-rpath /opt/nextage-open/lib'%(exename, f_code_name, d_tmp)
00043 print " - ",command
00044
00045 try:
00046 print 0
00047 print subprocess.check_call(command, shell=True)
00048 print subprocess.check_call(exename, shell=True)
00049
00050 f_exe = open(exename, 'r');
00051 f_txt = open('hrpiob_check_bin.py','w')
00052 f_txt.write('data="')
00053 f_txt.write(binascii.hexlify(f_exe.read()))
00054 f_txt.write('"')
00055 f_exe.close()
00056 f_txt.close()
00057 except Exception, e:
00058 ret = False
00059 print " ** create hrpiob check failed ...", e.message
00060
00061 print " create hrpiob check\t", ret, "\n"
00062
00063 shutil.rmtree(d_tmp)
00064 return ret
00065
00066
00067 iob_h = """
00068 /**
00069 * @file iob.h
00070 * @brief abstract interface for the robot hardware
00071 */
00072 #ifndef __IOB_H__
00073 #define __IOB_H__
00074
00075 #define ON 1
00076 #define OFF 0
00077
00078 #define MASK_ON 0
00079 #define MASK_OFF 1
00080
00081 /**
00082 * @name return value
00083 */
00084 //@{
00085 #ifndef FALSE
00086 #define FALSE 0
00087 #endif
00088
00089 #ifndef TRUE
00090 #define TRUE 1
00091 #endif
00092
00093 #define E_ID -1 ///< invalid joint(or sensor) id
00094 //@}
00095
00096 /**
00097 * @name servo alarm
00098 */
00099 //@{
00100 #define SS_OVER_VOLTAGE 0x001
00101 #define SS_OVER_LOAD 0x002
00102 #define SS_OVER_VELOCITY 0x004
00103 #define SS_OVER_CURRENT 0x008
00104
00105 #define SS_OVER_HEAT 0x010
00106 #define SS_TORQUE_LIMIT 0x020
00107 #define SS_VELOCITY_LIMIT 0x040
00108 #define SS_FORWARD_LIMIT 0x080
00109
00110 #define SS_REVERSE_LIMIT 0x100
00111 #define SS_POSITION_ERROR 0x200
00112 #define SS_ENCODER_ERROR 0x400
00113 #define SS_OTHER 0x800
00114 //@}
00115
00116 #define JID_ALL -1
00117 #define JID_INVALID -2
00118
00119 #ifdef __cplusplus
00120 extern "C"{
00121 #endif
00122
00123 typedef enum {
00124 JCM_FREE, ///< free
00125 JCM_POSITION, ///< position control
00126 JCM_TORQUE, ///< torque control
00127 JCM_VELOCITY, ///< velocity control
00128 JCM_NUM
00129 } joint_control_mode;
00130
00131 /**
00132 * @name the number of joints and sensors
00133 */
00134 // @{
00135 /**
00136 * @brief get the number of joints
00137 * @return the number of joints
00138 */
00139 int number_of_joints();
00140
00141 /**
00142 * @brief set the number of joints
00143 * @param num the number of joints
00144 * @return TRUE if the number of joints is set, FALSE otherwise
00145 */
00146 int set_number_of_joints(int num);
00147
00148 /**
00149 * @brief get the number of force sensors
00150 * @return the number of force sensors
00151 */
00152 int number_of_force_sensors();
00153
00154 /**
00155 * @brief set the number of force sensors
00156 * @param num the number of force sensors
00157 * @return TRUE if the number of force sensors is set, FALSE otherwise
00158 */
00159 int set_number_of_force_sensors(int num);
00160
00161 /**
00162 * @brief get the number of gyro sensors
00163 * @return the number of gyro sensors
00164 */
00165 int number_of_gyro_sensors();
00166
00167 /**
00168 * @brief set the number of gyro sensors
00169 * @param num the number of gyro sensors
00170 * @return TRUE if the number of gyro sensors is set, FALSE otherwise
00171 */
00172 int set_number_of_gyro_sensors(int num);
00173
00174 /**
00175 * @brief get the number of accelerometers
00176 * @return the number of accelerometers
00177 */
00178 int number_of_accelerometers();
00179
00180 /**
00181 * @brief set the number of accelerometers
00182 * @param num the number of accelerometers
00183 * @return TRUE if the number of accelerometers is set, FALSE otherwise
00184 */
00185 int set_number_of_accelerometers(int num);
00186
00187 /**
00188 * @brief get the number of attitude sensors
00189 * @return the number of attitude sensors
00190 */
00191 int number_of_attitude_sensors();
00192
00193 // @}
00194
00195 /**
00196 * @name joint angle
00197 */
00198 // @{
00199 /**
00200 * @brief read current joint angle[rad]
00201 * @param id joint id
00202 * @param angle actual joint angle[rad]
00203 * @retval TRUE this function is supported
00204 * @retval E_ID invalid joint id is specified
00205 * @retval FALSE otherwise
00206 */
00207 int read_actual_angle(int id, double *angle);
00208
00209 /**
00210 * @brief read array of current joint angles[rad]
00211 * @param angles array of joint angle[rad], length of array must be equal to number_of_joints()
00212 * @retval TRUE this function is supported
00213 * @retval FALSE otherwise
00214 */
00215 int read_actual_angles(double *angles);
00216
00217 /**
00218 * @brief read offset value for joint[rad]
00219 * @param id joint id
00220 * @param offset offset value[rad]
00221 * @retval TRUE offset value is read successfully
00222 * @retval E_ID invalid id is specified
00223 * @retval this function is not supported
00224 */
00225 int read_angle_offset(int id, double *offset);
00226
00227 /**
00228 * @brief write offset value for joint[rad]
00229 * @param id joint id
00230 * @param offset offset value[rad]
00231 * @retval TRUE offset values are written successfully
00232 * @retval E_ID invalid id is specified
00233 * @retval this function is not supported
00234 */
00235 int write_angle_offset(int id, double offset);
00236
00237 //@}
00238
00239 // @name joint
00240 // @{
00241 /**
00242 * @brief read power status of motor driver
00243 * @param id joint id
00244 * @param s ON or OFF is returned
00245 * @retval TRUE this function is supported
00246 * @retval E_ID invalid joint id is specified
00247 * @retval FALSE otherwise
00248 */
00249 int read_power_state(int id, int *s);
00250
00251 /**
00252 * @brief turn on/off power supply for motor driver
00253 * @param id joint id
00254 * @param com ON/OFF
00255 * @retval TRUE this function is supported
00256 * @retval E_ID invalid joint id is specified
00257 * @retval FALSE otherwise
00258 */
00259 int write_power_command(int id, int com);
00260
00261 /**
00262 * @brief turn on/off power supply for motor driver
00263 * @param id joint id
00264 * @param com ON/OFF
00265 * @retval TRUE this function is supported
00266 * @retval E_ID invalid joint id is specified
00267 * @retval FALSE otherwise
00268 */
00269 int read_power_command(int id, int *com);
00270
00271 /**
00272 * @brief read servo status
00273 * @param id joint id
00274 * @param s ON/OFF
00275 * @retval TRUE this function is supported
00276 * @retval E_ID invalid joint id is specified
00277 * @retval FALSE otherwise
00278 */
00279 int read_servo_state(int id, int *s);
00280
00281 /**
00282 * @brief read servo alarms
00283 * @param id joint id
00284 * @param a servo alarms
00285 * @retval TRUE this function is supported
00286 * @retval E_ID invalid joint id is specified
00287 * @retval FALSE otherwise
00288 */
00289 int read_servo_alarm(int id, int *a);
00290
00291 /**
00292 * @brief read joint control mode
00293 * @param id joint id
00294 * @param s joint control mode
00295 * @retval TRUE this function is supported
00296 * @retval E_ID invalid joint id is specified
00297 * @retval FALSE otherwise
00298 */
00299 int read_control_mode(int id, joint_control_mode *s);
00300
00301 /**
00302 * @brief write joint control mode
00303 * @param id joint id
00304 * @param s joint control mode
00305 * @retval TRUE this function is supported
00306 * @retval E_ID invalid joint id is specified
00307 * @retval FALSE otherwise
00308 */
00309 int write_control_mode(int id, joint_control_mode s);
00310
00311 /**
00312 * @brief read array of current joint torques[Nm]
00313 * @param torques array of actual joint torque[Nm], length of array must be equal to number_of_joints()
00314 * @retval TRUE this function is supported
00315 * @retval FALSE otherwise
00316 */
00317 int read_actual_torques(double *torques);
00318
00319 /**
00320 * @brief read command torque[Nm]
00321 * @param id joint id
00322 * @param torque joint torque[Nm]
00323 * @retval TRUE this function is supported
00324 * @retval E_ID invalid joint id is specified
00325 * @retval FALSE otherwise
00326 */
00327 int read_command_torque(int id, double *torque);
00328
00329 /**
00330 * @brief write command torque[Nm]
00331 * @param id joint id
00332 * @param torque joint torque[Nm]
00333 * @return TRUE if this function is supported, FALSE otherwise
00334 */
00335 int write_command_torque(int id, double torque);
00336
00337 /**
00338 * @brief read array of command torques[Nm]
00339 * @param torques array of command torques[Nm]
00340 * @retval TRUE this function is supported
00341 * @retval FALSE otherwise
00342 */
00343 int read_command_torques(double *torques);
00344
00345 /**
00346 * @brief write array of command torques[Nm]
00347 * @param torques array of command torques[Nm]
00348 * @retval TRUE this function is supported
00349 * @retval FALSE otherwise
00350 */
00351 int write_command_torques(const double *torques);
00352
00353 /**
00354 * @brief read command angle[rad]
00355 * @param id joint id
00356 * @param angle command joint angle[rad]
00357 * @retval TRUE this function is supported
00358 * @retval FALSE otherwise
00359 */
00360 int read_command_angle(int id, double *angle);
00361
00362 /**
00363 * @brief write command angle[rad]
00364 * @param id joint id
00365 * @param angle command joint angle[rad]
00366 * @return TRUE or E_ID
00367 */
00368 int write_command_angle(int id, double angle);
00369
00370 /**
00371 * @brief read array of command angles[rad]
00372 * @param angles array of joint angles[rad], length of array must equal to DOF
00373 * @retval TRUE this function is supported
00374 * @retval FALSE otherwise
00375 */
00376 int read_command_angles(double *angles);
00377
00378 /**
00379 * @brief write array of command angles[rad]
00380 * @param angles array of joint angles[rad], length of array must equal to DOF
00381 * @retval TRUE this function is supported
00382 * @retval FALSE otherwise
00383 */
00384 int write_command_angles(const double *angles);
00385
00386 /**
00387 * @brief read P gain[Nm/rad]
00388 * @param id joint id
00389 * @param gain P gain[Nm/rad]
00390 * @return TRUE or E_ID
00391 */
00392 int read_pgain(int id, double *gain);
00393
00394 /**
00395 * @brief write P gain[Nm/rad]
00396 * @param id joint id
00397 * @param gain P gain[Nm/rad]
00398 * @return TRUE or E_ID
00399 */
00400 int write_pgain(int id, double gain);
00401
00402 /**
00403 * @brief read D gain[Nm/(rad/s)]
00404 * @param id joint id
00405 * @param gain D gain[Nm/(rad/s)]
00406 * @return TRUE or E_ID
00407 */
00408 int read_dgain(int id, double *gain);
00409
00410 /**
00411 * @brief write D gain[Nm/(rad/s)]
00412 * @param id joint id
00413 * @param gain D gain[Nm/(rad/s)]
00414 * @return TRUE or E_ID
00415 */
00416 int write_dgain(int id, double gain);
00417
00418 /**
00419 * @brief read actual angular velocity[rad/s]
00420 * @param id joint id
00421 * @param vel angular velocity [rad/s]
00422 * @return TRUE or E_ID
00423 */
00424 int read_actual_velocity(int id, double *vel);
00425
00426 /**
00427 * @brief read command angular velocity[rad/s]
00428 * @param id joint id
00429 * @param vel angular velocity [rad/s]
00430 * @return TRUE or E_ID
00431 */
00432 int read_command_velocity(int id, double *vel);
00433
00434 /**
00435 * @brief write command angular velocity[rad/s]
00436 * @param id joint id
00437 * @param vel angular velocity [rad/s]
00438 * @return TRUE or E_ID
00439 */
00440 int write_command_velocity(int id, double vel);
00441
00442 /**
00443 * @brief read actual angular velocities[rad/s]
00444 * @param vels array of angular velocity [rad/s]
00445 * @retval TRUE this function is supported
00446 * @retval FALSE otherwise
00447 */
00448 int read_actual_velocities(double *vels);
00449
00450 /**
00451 * @brief read command angular velocities[rad/s]
00452 * @param vels array of angular velocity [rad/s]
00453 * @retval TRUE this function is supported
00454 * @retval FALSE otherwise
00455 */
00456 int read_command_velocities(double *vels);
00457
00458 /**
00459 * @brief write command angular velocities[rad/s]
00460 * @param vels array of angular velocity [rad/s]
00461 * @retval TRUE this function is supported
00462 * @retval FALSE otherwise
00463 */
00464 int write_command_velocities(const double *vels);
00465
00466 /**
00467 * @brief turn on/off joint servo
00468 * @param id joint id
00469 * @param com ON/OFF
00470 * @return TRUE if this function is supported, FALSE otherwise
00471 */
00472 int write_servo(int id, int com);
00473
00474 /**
00475 * @brief read temperature of motor driver[Celsius]
00476 * @param id joint id
00477 * @param v temperature[Celsius]
00478 * @retval TRUE temperature is read successfully
00479 * @retval E_ID invalid joint id is specified
00480 * @retval FALSE this function is not supported
00481 */
00482 int read_driver_temperature(int id, unsigned char* v);
00483
00484 /**
00485 * @brief read callibration state of joint
00486 * @param id joint id
00487 * @param s TRUE if calibration is already done, FALSE otherwise
00488 * @retval TRUE calibration status is read successfully
00489 * @retval E_ID invalid joint id is specified
00490 * @retval FALSE this function is not supported
00491 */
00492 int read_calib_state(int id, int *s);
00493
00494 /**
00495 * @brief get length of extra servo states
00496 * @param id joint id
00497 * @return length of extra servo states
00498 */
00499 size_t length_of_extra_servo_state(int id);
00500
00501 /**
00502 * @brief read extra servo states
00503 * @param id joint id
00504 * @param state array of int where extra servo states are stored
00505 * @return TRUE if read successfully, FALSE otherwise
00506 */
00507 int read_extra_servo_state(int id, int *state);
00508
00509 //@}
00510
00511 /**
00512 * @name force sensor
00513 */
00514 //@{
00515 /**
00516 * @brief read output of force sensor
00517 * @param id Force Sensor id
00518 * @param forces array of forces[N] and moments[Nm], length of array must be 6
00519 * @return TRUE or E_ID
00520 */
00521 int read_force_sensor(int id, double *forces);
00522
00523 /**
00524 * @brief read offset values for force sensor output
00525 * @param id force/torque sensor id
00526 * @param offsets offset values[N][Nm], length of array must be 6.
00527 * @retval TRUE offset values are read successfully
00528 * @retval E_ID invalid id is specified
00529 * @retval this function is not supported
00530 */
00531 int read_force_offset(int id, double *offsets);
00532
00533 /**
00534 * @brief write offset values for forcesensor output
00535 * @param id accelerometer id
00536 * @param offsets offset values[N][Nm], length of array must be 6.
00537 * @retval TRUE offset values are written successfully
00538 * @retval E_ID invalid id is specified
00539 * @retval this function is not supported
00540 */
00541 int write_force_offset(int id, double *offsets);
00542 //@}
00543
00544 /**
00545 * @name gyro sensor
00546 */
00547 //@{
00548 /**
00549 * @brief read output of gyro sensor
00550 * @param id gyro sensor id
00551 * @param rates angular velocities [rad/s], length of array must be 3
00552 * @return TRUE or E_ID
00553 */
00554 int read_gyro_sensor(int id, double *rates);
00555
00556 /**
00557 * @brief read offset values for gyro sensor output
00558 * @param id gyro sensor id
00559 * @param offset offset values[rad/s], length of array must be 3.
00560 * @retval TRUE offset values are read successfully
00561 * @retval E_ID invalid id is specified
00562 * @retval this function is not supported
00563 */
00564 int read_gyro_sensor_offset(int id, double *offset);
00565
00566 /**
00567 * @brief write offset values for gyro sensor output
00568 * @param id gyro sensor id
00569 * @param offset offset values[rad/s], length of array must be 3.
00570 * @retval TRUE offset values are written successfully
00571 * @retval E_ID invalid id is specified
00572 * @retval this function is not supported
00573 */
00574 int write_gyro_sensor_offset(int id, double *offset);
00575
00576 //@}
00577
00578 /**
00579 * @name acceleromter
00580 */
00581 //@{
00582 /**
00583 * @brief read output of accelerometer
00584 * @param id accelerometer id
00585 * @param accels accelerations [m/s^2], length of array must be 3
00586 * @return TRUE or E_ID
00587 */
00588 int read_accelerometer(int id, double *accels);
00589
00590 /**
00591 * @brief read offset values for accelerometer output
00592 * @param id accelerometer id
00593 * @param offset offset values[rad/s^2], length of array must be 3.
00594 * @retval TRUE offset values are read successfully
00595 * @retval E_ID invalid id is specified
00596 * @retval this function is not supported
00597 */
00598 int read_accelerometer_offset(int id, double *offset);
00599
00600 /**
00601 * @brief write offset values for accelerometer output
00602 * @param id accelerometer id
00603 * @param offset offset values[rad/s^2], length of array must be 3.
00604 * @retval TRUE offset values are written successfully
00605 * @retval E_ID invalid id is specified
00606 * @retval this function is not supported
00607 */
00608 int write_accelerometer_offset(int id, double *offset);
00609
00610 //@}
00611
00612 /**
00613 * @name attitude sensor
00614 */
00615 //@{
00616 /**
00617 * @brief read output of attitude sensor
00618 * @param id attitude sensor id
00619 * @param att roll-pitch-yaw angle[rad], length of array must be 3
00620 * @retval TRUE sensor values are read successfully
00621 * @retval E_ID invalid id is specified
00622 * @retval this function is not supported
00623 */
00624 int read_attitude_sensor(int id, double *att);
00625
00626 int write_attitude_sensor_offset(int id, double *offset);
00627 //@}
00628
00629 /**
00630 * @name power supply
00631 */
00632 //@{
00633 /**
00634 * @brief read status of power source
00635 * @param v voltage[V]
00636 * @param a current[A]
00637 * @return TRUE or FALSE
00638 */
00639 int read_power(double *v, double *a);
00640 //@}
00641
00642 /**
00643 * @name thermometer
00644 */
00645 //@{
00646 /**
00647 * @brief read thermometer
00648 * @param id id of thermometer
00649 * @param v temperature[Celsius]
00650 * @retval TRUE temperature is read successfully
00651 * @retval E_ID invalid thermometer id is specified
00652 * @retval this function is not supported
00653 */
00654 int read_temperature(int id, double *v);
00655 //@}
00656
00657 /**
00658 * @name open/close
00659 */
00660 //@{
00661 /**
00662 * @brief open connection with joint servo process
00663 * @retval TRUE opened successfully
00664 * @retval FALSE otherwise
00665 */
00666 int open_iob(void);
00667
00668 /**
00669 * @brief close connection with joint servo process
00670 * @retval TRUE closed successfully
00671 * @retval FALSE otherwise
00672 */
00673 int close_iob(void);
00674
00675 int reset_body(void);
00676
00677 /**
00678 * @brief lock access to iob
00679 * @retval TRUE iob is locked successfully
00680 * @retval FALSE some other process is locking iob
00681 */
00682 int lock_iob();
00683
00684 /**
00685 * @brief unlock access to iob
00686 */
00687 int unlock_iob();
00688
00689 /**
00690 * @brief read id of the process whic is locking access to iob
00691 */
00692 int read_lock_owner(pid_t *pid);
00693
00694 /**
00695 * @brief
00696 */
00697 unsigned long long read_iob_frame();
00698
00699 /**
00700 * @brief
00701 * @return the number of substeps
00702 */
00703 int number_of_substeps();
00704
00705 /**
00706 * @brief wait until iob signal is issued
00707 * @return TRUE if signal is received successfully, FALSE otherwise
00708 */
00709 int wait_for_iob_signal();
00710
00711 /**
00712 * @brief set the period of signals issued by wait_for_iob_signal()
00713 * @param period_ns the period of signals[ns]
00714 * @return TRUE if set successfully, FALSE otherwise
00715 */
00716 int set_signal_period(long period_ns);
00717
00718 /**
00719 * @brief get the period of signals issued by wait_for_iob_signal()
00720 * @return the period of signals[ns]
00721 */
00722 long get_signal_period();
00723
00724 /**
00725 * @brief initialize joint angle
00726 * @param name joint name, part name or "all"
00727 * @param option string of joint angle initialization
00728 * @return TRUE if initialized successfully, FALSE otherwise
00729 */
00730 int initializeJointAngle(const char *name, const char *option);
00731
00732 /**
00733 * @brief read_digital_input, non-applicable bits are nop
00734 * @param dinput digital input from environment
00735 * @return TRUE if applicable, FALSE otherwise
00736 */
00737 int read_digital_input(char *dinput);
00738
00739 /**
00740 * @brief get_digital_input_length
00741 * @return length of digital input in bytes
00742 */
00743 int length_digital_input();
00744
00745 /**
00746 * @brief write_digital_output, non-applicable bits are nop
00747 * @param doutput set digital output to environment
00748 * @return TRUE if applicable, FALSE otherwise
00749 */
00750 int write_digital_output(const char *doutput);
00751
00752 /**
00753 * @brief write_digital_output, non-applicable bits are nop
00754 * @param doutput set digital output to environment
00755 * @param mask binary vector which selects output to be set
00756 * @return TRUE if applicable, FALSE otherwise
00757 */
00758 int write_digital_output_with_mask(const char *doutput, const char *dmask);
00759
00760 /**
00761 * @brief get_digital_output_length
00762 * @return length of digital output in bytes
00763 */
00764 int length_digital_output();
00765
00766 /**
00767 * @brief read_digital_output, non-applicable bits are nop
00768 * @param doutput digital output to environment
00769 * @return TRUE if applicable, FALSE otherwise
00770 */
00771 int read_digital_output(char *doutput);
00772 //@}
00773
00774 #ifdef __cplusplus
00775 }
00776 #endif
00777
00778 #endif
00779
00780 """
00781
00782
00783
00784 if __name__ == '__main__':
00785 create_check_hrpiob_iob()
00786
00787