| 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 |
}
|