Revision 14831938 examples/yarp_icub/src/icub_jointinterface.cpp
| examples/yarp_icub/src/icub_jointinterface.cpp | ||
|---|---|---|
| 39 | 39 |
iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface() {
|
| 40 | 40 |
scope = _scope; |
| 41 | 41 |
|
| 42 |
// running in sim? |
|
| 43 |
if (scope == "/icubSim") {
|
|
| 44 |
cout << "> icub simulation detected\n"; |
|
| 45 |
running_in_simulation_ = true; |
|
| 46 |
} else {
|
|
| 47 |
running_in_simulation_ = false; |
|
| 48 |
} |
|
| 49 |
|
|
| 50 |
|
|
| 42 | 51 |
// add mappings from icub ids to humotion ids |
| 43 | 52 |
init_id_map(); |
| 44 | 53 |
|
| ... | ... | |
| 58 | 67 |
// fetch yarp views: |
| 59 | 68 |
bool success = true; |
| 60 | 69 |
// success &= yarp_polydriver_.view(yarp_iencs_); |
| 61 |
// success &= yarp_polydriver_.view(yarp_ipos_);
|
|
| 70 |
success &= yarp_polydriver_.view(yarp_ipos_); |
|
| 62 | 71 |
success &= yarp_polydriver_.view(yarp_ivel_); |
| 63 | 72 |
success &= yarp_polydriver_.view(yarp_ilimits_); |
| 64 | 73 |
success &= yarp_polydriver_.view(yarp_pid_); |
| ... | ... | |
| 79 | 88 |
|
| 80 | 89 |
//! destructor |
| 81 | 90 |
iCubJointInterface::~iCubJointInterface() {
|
| 82 |
// stop velocity controller on exit
|
|
| 91 |
// stop controller on exit |
|
| 83 | 92 |
yarp_ivel_->stop(); |
| 93 |
yarp_ipos_->stop(); |
|
| 84 | 94 |
} |
| 85 | 95 |
|
| 86 | 96 |
//! init the controller that allows to write target angles or velocities |
| 87 | 97 |
void iCubJointInterface::init_controller() {
|
| 88 | 98 |
int number_of_joints; |
| 89 | 99 |
|
| 90 |
// use position controller, first fetch no of axes: |
|
| 91 |
yarp_ivel_->getAxes(&number_of_joints); |
|
| 100 |
if (running_in_simulation_) {
|
|
| 101 |
// running in simulation, use position control mode as velocity seems unsupported right now |
|
| 102 |
yarp_ipos_->getAxes(&number_of_joints); |
|
| 103 |
|
|
| 104 |
// prepare to set ref accel |
|
| 105 |
// yarp_commands_.resize(number_of_joints); |
|
| 106 |
// yarp_commands_ = 200000.0; |
|
| 107 |
// yarp_ipos_->setRefAccelerations(yarp_commands_.data()); |
|
| 92 | 108 |
|
| 93 |
// set ref acceleration to a value for all axes: |
|
| 94 |
yarp_commands_.resize(number_of_joints); |
|
| 95 |
yarp_commands_ = 1e6; |
|
| 96 |
yarp_ivel_->setRefAccelerations(yarp_commands_.data()); |
|
| 97 |
yarp_ivel_->setVelocityMode(); |
|
| 109 |
// enable position control |
|
| 110 |
yarp_ipos_->setPositionMode(); |
|
| 111 |
} else {
|
|
| 112 |
// use velocity controller, first fetch no of axes: |
|
| 113 |
yarp_ivel_->getAxes(&number_of_joints); |
|
| 114 |
|
|
| 115 |
// set ref acceleration to a value for all axes: |
|
| 116 |
yarp_commands_.resize(number_of_joints); |
|
| 117 |
yarp_commands_ = 1e6; |
|
| 118 |
yarp_ivel_->setRefAccelerations(yarp_commands_.data()); |
|
| 119 |
|
|
| 120 |
// finally enable velocity control mode |
|
| 121 |
yarp_ivel_->setVelocityMode(); |
|
| 122 |
} |
|
| 98 | 123 |
} |
| 99 | 124 |
|
| 100 | 125 |
//! initialise icub joint id to humotion joint id mappings |
| ... | ... | |
| 302 | 327 |
<< position_error << " " |
| 303 | 328 |
<< " PID" << icub_id << "\n";*/ |
| 304 | 329 |
|
| 330 |
|
|
| 331 |
if (icub_id > ICUB_ID_EYES_VERGENCE) {
|
|
| 332 |
cerr << "> ERROR: set_target_positionmode(id=" << icub_id << ", ...) not supported\n"; |
|
| 333 |
return; |
|
| 334 |
} |
|
| 335 |
|
|
| 336 |
|
|
| 305 | 337 |
// execute velocity move |
| 306 | 338 |
bool success; |
| 307 | 339 |
int count = 0; |
| 308 | 340 |
do {
|
| 309 |
success = yarp_ivel_->velocityMove(icub_id, pv_mix_velocity); |
|
| 341 |
if (running_in_simulation_) {
|
|
| 342 |
// running in sim, use position control |
|
| 343 |
success = yarp_ipos_->positionMove(icub_id, target_angle_[icub_id]); |
|
| 344 |
} else {
|
|
| 345 |
// use smooth velocity control on real robot |
|
| 346 |
success = yarp_ivel_->velocityMove(icub_id, pv_mix_velocity); |
|
| 347 |
} |
|
| 348 |
|
|
| 310 | 349 |
if (count++ >= 10) {
|
| 311 |
cerr << "ERROR: failed to send velocity move command! gave up after 10 trials\n";
|
|
| 350 |
cerr << "ERROR: failed to send motion command! gave up after 10 trials\n";
|
|
| 312 | 351 |
yarp_ivel_->stop(); |
| 352 |
yarp_ipos_->stop(); |
|
| 313 | 353 |
exit(EXIT_FAILURE); |
| 314 | 354 |
} |
| 315 | 355 |
} while (!success); |
| ... | ... | |
| 393 | 433 |
*/ |
| 394 | 434 |
|
| 395 | 435 |
if ((icub_jointid != -1) && (icub_jointid <= ICUB_ID_EYES_VERGENCE)) {
|
| 396 |
if (enable) {
|
|
| 397 |
yarp_amp_->enableAmp(icub_jointid); |
|
| 398 |
yarp_pid_->enablePid(icub_jointid); |
|
| 399 |
} else {
|
|
| 400 |
yarp_pid_->disablePid(icub_jointid); |
|
| 401 |
yarp_amp_->disableAmp(icub_jointid); |
|
| 436 |
if (!running_in_simulation_) {
|
|
| 437 |
// only set amp status on real robot. simulation crashes during this call ... :-X |
|
| 438 |
if (enable) {
|
|
| 439 |
yarp_amp_->enableAmp(icub_jointid); |
|
| 440 |
yarp_pid_->enablePid(icub_jointid); |
|
| 441 |
} else {
|
|
| 442 |
yarp_pid_->disablePid(icub_jointid); |
|
| 443 |
yarp_amp_->disableAmp(icub_jointid); |
|
| 444 |
} |
|
| 402 | 445 |
} |
| 403 | 446 |
} |
| 404 | 447 |
} |
Also available in: Unified diff