00001
00002
00003
00004
00005
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include <cstdio>
00024 #include <dlfcn.h>
00025
00026
00027 #include <unistd.h>
00028 #include <pthread.h>
00029
00030 #include <hrpsys/io/iob.h>
00031 #include <nextage-open.hpp>
00032
00033 #define JID_INVALID -2
00034
00035
00036 static NEXTAGE_OPEN::OpenIFv10 *nxifv1 = NULL;
00037
00038 #define CHECK_JOINT_ID(id) if((id) < 0 || (id) >= number_of_joints()) return E_ID
00039 #define CHECK_JOINT_ID_DOF(id) if((id) < 0 || (id) >= DOF) return E_ID
00040
00042
00043 int number_of_joints()
00044 {
00045 return (15);
00046 }
00047
00049
00050 int set_number_of_joints(int num)
00051 {
00052 return FALSE;
00053 }
00054
00056
00057 int number_of_force_sensors()
00058 {
00059 return 0;
00060 }
00061
00063
00064 int set_number_of_force_sensors(int num)
00065 {
00066 return FALSE;
00067 }
00068
00070
00071 int number_of_gyro_sensors()
00072 {
00073 return 0;
00074 }
00075
00077
00078 int set_number_of_gyro_sensors(int num)
00079 {
00080 return FALSE;
00081 }
00082
00084
00085 int number_of_accelerometers()
00086 {
00087 return 0;
00088 }
00089
00091
00092 int set_number_of_accelerometers(int num)
00093 {
00094 return FALSE;
00095 }
00096
00098
00099 int number_of_attitude_sensors()
00100 {
00101 return 0;
00102 }
00103
00105
00106
00107 int read_actual_angle(int id, double *angle)
00108 {
00109 if (nxifv1)
00110 return nxifv1->read_actual_angle(id, angle);
00111 else
00112 return FALSE;
00113 }
00114
00116
00117 int read_actual_angles(double *angles)
00118 {
00119 if (nxifv1)
00120 return nxifv1->read_actual_angles(angles);
00121 else
00122 return FALSE;
00123 }
00124
00126
00127 int read_angle_offset(int id, double *angle)
00128 {
00129 return FALSE;
00130 }
00131
00133
00134 int write_angle_offset(int id, double angle)
00135 {
00136 return FALSE;
00137 }
00138
00139
00140
00142
00143 int read_power_state(int id, int *s)
00144 {
00145 if (nxifv1)
00146 return nxifv1->read_power_state(id, s);
00147 else
00148 return FALSE;
00149 }
00150
00151
00152
00153 int write_power_command(int id, int com)
00154 {
00155 if (nxifv1)
00156 return nxifv1->write_power_command(id, com);
00157 else
00158 return FALSE;
00159 }
00160
00162
00163 int read_power_command(int id, int *com)
00164 {
00165 if (nxifv1)
00166 return nxifv1->read_power_command(id, com);
00167 else
00168 return FALSE;
00169 }
00170
00172
00173 int read_servo_state(int id, int *s)
00174 {
00175 if (nxifv1)
00176 return nxifv1->read_servo_state(id, s);
00177 else
00178 return FALSE;
00179 }
00180
00182
00183 int read_servo_alarm(int id, int *a)
00184 {
00185 if (nxifv1)
00186 return nxifv1->read_servo_alarm(id, a);
00187 else
00188 return FALSE;
00189 }
00190
00192
00193 int read_control_mode(int id, joint_control_mode *s)
00194 {
00195 return FALSE;
00196 }
00197
00199
00200 int write_control_mode(int id, joint_control_mode s)
00201 {
00202 return FALSE;
00203 }
00204
00206
00207 int read_actual_torque(int id, double *angle)
00208 {
00209 return FALSE;
00210 }
00211
00213
00214 int read_actual_torques(double *torques)
00215 {
00216 return FALSE;
00217 }
00218
00220
00221 int read_command_torque(int id, double *torque)
00222 {
00223 return FALSE;
00224 }
00225
00227
00228 int write_command_torque(int id, double torque)
00229 {
00230 return FALSE;
00231 }
00232
00234
00235 int read_command_torques(double *torques)
00236 {
00237 return FALSE;
00238 }
00239
00240 int write_command_torques(const double *torques)
00241 {
00242 return FALSE;
00243 }
00244
00245
00247
00248 int read_command_angle(int id, double *angle)
00249 {
00250 if (nxifv1)
00251 return nxifv1->read_command_angle(id, angle);
00252 else
00253 return FALSE;
00254 }
00255
00257
00258 int read_command_angles(double *angles)
00259 {
00260 if (nxifv1)
00261 return nxifv1->read_command_angles(angles);
00262 else
00263 return FALSE;
00264 }
00265
00267
00268 int write_command_angle(int id, double angle)
00269 {
00270 if (nxifv1)
00271 return nxifv1->write_command_angle(id, angle);
00272 else
00273 return FALSE;
00274 }
00275
00277
00278 int write_command_angles(const double *angles)
00279 {
00280 if (nxifv1)
00281 return nxifv1->write_command_angles(angles);
00282 else
00283 return FALSE;
00284 }
00285
00287
00288 int read_pgain(int id, double *gain)
00289 {
00290 return E_ID;
00291 }
00292
00294
00295 int write_pgain(int id, double gain)
00296 {
00297 return E_ID;
00298 }
00299
00301
00302 int read_dgain(int id, double *gain)
00303 {
00304 return E_ID;
00305 }
00306
00308
00309
00310 int write_dgain(int id, double gain)
00311 {
00312 return E_ID;
00313 }
00314
00315
00316
00317
00318 int read_actual_velocity(int id, double *vel)
00319 {
00320 return FALSE;
00321 }
00322
00324
00325 int read_command_velocity(int id, double *vel)
00326 {
00327 return E_ID;
00328 }
00329
00331
00332 int write_command_velocity(int id, double vel)
00333 {
00334 return E_ID;
00335 }
00336
00338
00339 int read_command_velocities(double *vels)
00340 {
00341 return FALSE;
00342 }
00343
00344 int read_actual_velocities(double *vels)
00345 {
00346 return FALSE;
00347 }
00348
00350
00351 int write_command_velocities(const double *vels)
00352 {
00353 return FALSE;
00354 }
00355
00357
00358 int write_servo(int id, int com)
00359 {
00360 if (nxifv1)
00361 return nxifv1->write_servo(id, com);
00362 else
00363 return FALSE;
00364 }
00365
00367
00368 int read_driver_temperature(int id, unsigned char *v)
00369 {
00370 if (nxifv1)
00371 return nxifv1->read_driver_temperature(id, v);
00372 else
00373 return FALSE;
00374 }
00375
00377
00378 int read_calib_state(int id, int *s)
00379 {
00380 if (nxifv1)
00381 return nxifv1->read_calib_state(id, s);
00382 else
00383 return FALSE;
00384 }
00385
00387
00388 size_t length_of_extra_servo_state(int id)
00389 {
00390 return 0;
00391 }
00392
00394
00395 int read_extra_servo_state(int id, int *state)
00396 {
00397 return TRUE;
00398 }
00399
00401
00402 int read_force_sensor(int id, double *forces)
00403 {
00404 return E_ID;
00405 }
00406
00408
00409 int read_force_offset(int id, double *offsets)
00410 {
00411 return E_ID;
00412 }
00413
00415
00416 int write_force_offset(int id, double *offsets)
00417 {
00418 return E_ID;
00419 }
00420
00422
00423
00424 int read_gyro_sensor(int id, double *rates)
00425 {
00426 return E_ID;
00427 }
00428
00430
00431 int read_gyro_sensor_offset(int id, double *offset)
00432 {
00433 return E_ID;
00434 }
00435
00437
00438 int write_gyro_sensor_offset(int id, double *offset)
00439 {
00440 return E_ID;
00441 }
00442
00444
00445
00446 int read_accelerometer(int id, double *accels)
00447 {
00448 return E_ID;
00449 }
00450
00452
00453 int read_accelerometer_offset(int id, double *offset)
00454 {
00455 return E_ID;
00456 }
00457
00459
00460 int write_accelerometer_offset(int id, double *offset)
00461 {
00462 return E_ID;
00463 }
00464
00466
00467 int read_attitude_sensor(int id, double *att)
00468 {
00469 return E_ID;
00470 }
00471
00473
00474 int write_attitude_sensor_offset(int id, double *offset)
00475 {
00476 return E_ID;
00477 }
00478
00480
00481 int read_power(double *voltage, double *current)
00482 {
00483 if (nxifv1)
00484 return nxifv1->read_power(voltage, current);
00485 else
00486 return FALSE;
00487 }
00488
00490
00491
00492 int read_temperature(int id, double *v)
00493 {
00494 return E_ID;
00495 }
00496
00497
00498 static pthread_mutex_t openLock = PTHREAD_MUTEX_INITIALIZER;
00499
00500
00501 int open_iob(void)
00502 {
00503 pthread_mutex_lock (&openLock);
00504 if (nxifv1 == NULL) {
00505 void *lib = dlopen("libNxOpenCore.so", RTLD_LOCAL|RTLD_NOW);
00506 if (lib == NULL) {
00507 std::fprintf(stdout, "** failed in dlopen(libNxOpenCore.so): %s\n", dlerror());
00508 pthread_mutex_unlock(&openLock);
00509 return FALSE;
00510 }
00511
00512 bool (*check)(const char *);
00513 check = reinterpret_cast<bool (*)(const char *)>(dlsym(lib, "nextage_open_supported"));
00514 if (check==NULL) {
00515 std::fprintf(stdout, "** failed to find nextage_open_supported(): %s\n", dlerror());
00516 pthread_mutex_unlock(&openLock);
00517 return FALSE;
00518 }
00519
00520 if (check("v1.0")==false) {
00521 std::fprintf(stdout, "** v1.0 not supported\n");
00522 pthread_mutex_unlock(&openLock);
00523 return FALSE;
00524 }
00525
00526 NEXTAGE_OPEN::OpenIFv10 * (*getif)(void);
00527 getif = reinterpret_cast<NEXTAGE_OPEN::OpenIFv10 * (*)(void)>(dlsym(lib, "nextage_open_getIFv10"));
00528 if (getif==NULL) {
00529 std::fprintf(stdout, "** failed to find nextage_open_getIFv10(): %s\n", dlerror());
00530 pthread_mutex_unlock(&openLock);
00531 return FALSE;
00532 }
00533
00534 nxifv1 = getif();
00535 if (nxifv1 == NULL) {
00536 std::fprintf(stdout, "** failed to getIF()\n");
00537 pthread_mutex_unlock(&openLock);
00538 return FALSE;
00539 }
00540
00541
00542 std::fprintf(stdout, "open_iob - NEXTAGE OPEN I/F v1 instance at 0x%x\n", nxifv1);
00543 pthread_mutex_unlock(&openLock);
00544 return nxifv1->open_iob();
00545
00546 } else {
00547 std::fprintf(stdout, "open_iob - NEXTAGE OPEN I/F instance is not NULL, ignored\n");
00548 pthread_mutex_unlock(&openLock);
00549 return TRUE;
00550 }
00551
00552 pthread_mutex_unlock(&openLock);
00553 return FALSE;
00554 }
00555
00556
00557
00558 int close_iob(void)
00559 {
00560
00561 return TRUE;
00562 #if 0
00563 if (nxifv1 != NULL) {
00564 int res=nxifv1->close_iob();
00565 delete nxifv1;
00566 nxifv1 = NULL;
00567 return res;
00568 } else {
00569 std::fprintf(stdout, "close_iob(): nxifv1 is NULL\n");
00570 }
00571
00572 return FALSE;
00573 #endif
00574 }
00575
00576
00577
00578 int reset_body(void)
00579 {
00580 if (nxifv1)
00581 return nxifv1->reset_body();
00582 else
00583 return FALSE;
00584 }
00585
00586
00587 int lock_iob()
00588 {
00589 if (nxifv1)
00590 return nxifv1->lock_iob();
00591 else
00592 return FALSE;
00593 }
00594
00595
00596
00597
00598 int unlock_iob()
00599 {
00600 if (nxifv1)
00601 return nxifv1->unlock_iob();
00602 else
00603 return FALSE;
00604 }
00605
00606
00607 int read_lock_owner(pid_t *pid)
00608 {
00609 if (nxifv1)
00610 return nxifv1->read_lock_owner(pid);
00611 else
00612 return FALSE;
00613 }
00614
00615
00616
00617
00618 unsigned long long read_iob_frame()
00619 {
00620 if (nxifv1)
00621 return nxifv1->read_iob_frame();
00622 else
00623 return FALSE;
00624 }
00625
00626
00627
00628
00629 int number_of_substeps()
00630 {
00631 if (nxifv1)
00632 return nxifv1->number_of_substeps();
00633 else
00634 return FALSE;
00635
00636
00637 }
00638
00639
00640 int wait_for_iob_signal()
00641 {
00642 if (nxifv1)
00643 return nxifv1->wait_for_iob_signal();
00644 else
00645 return FALSE;
00646 }
00647
00653 int set_signal_period(long period_ns)
00654 {
00655 return FALSE;
00656 }
00657
00662 long get_signal_period()
00663 {
00664 if (nxifv1)
00665 return nxifv1->get_signal_period();
00666 else
00667 return FALSE;
00668
00669 }
00670
00671
00672
00674
00675
00676
00677
00678
00679
00680
00681 int initializeJointAngle(const char *name, const char *option)
00682 {
00683 if (nxifv1)
00684 return nxifv1->initializeJointAngle(name, option);
00685 else
00686 return FALSE;
00687 }
00688
00689 int read_digital_input(char *dIn)
00690 {
00691 if (nxifv1)
00692 return nxifv1->read_digital_input(dIn);
00693 else
00694 return FALSE;
00695 }
00696
00697 int length_digital_input()
00698 {
00699 if (nxifv1)
00700 return nxifv1->length_digital_input();
00701 else
00702 return FALSE;
00703 }
00704
00705 int write_digital_output(const char *doutput)
00706 {
00707 if (nxifv1)
00708 return nxifv1->write_digital_output(doutput);
00709 else
00710 return FALSE;
00711 }
00712
00713 int write_digital_output_with_mask(const char *doutput, const char *mask)
00714 {
00715 if (nxifv1)
00716 return nxifv1->write_digital_output_with_mask(doutput, mask);
00717 else
00718 return FALSE;
00719 }
00720
00721 int length_digital_output()
00722 {
00723 if (nxifv1)
00724 return nxifv1->length_digital_output();
00725 else
00726 return FALSE;
00727 }
00728
00729 int read_digital_output(char *doutput)
00730 {
00731 if (nxifv1)
00732 return nxifv1->read_digital_output(doutput);
00733 else
00734 return FALSE;
00735 }