Revision 372eec67 examples/yarp_icub/src/icub_jointinterface.cpp
| examples/yarp_icub/src/icub_jointinterface.cpp | ||
|---|---|---|
| 2 | 2 |
#include "icub_faceinterface.h" |
| 3 | 3 |
|
| 4 | 4 |
#include <yarp/os/Property.h> |
| 5 |
using namespace yarp::dev; |
|
| 6 |
using namespace yarp::sig; |
|
| 7 |
using namespace yarp::os; |
|
| 8 |
using namespace std; |
|
| 9 |
/*running: |
|
| 10 |
/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simConfig:> iCub_SIM |
|
| 11 |
/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simFaceExpressions:> ../../../../bin/simFaceExpressions |
|
| 12 |
yarp connect /face/eyelids /icubSim/face/eyelids |
|
| 13 |
yarp connect /face/image/out /icubSim/texture/face |
|
| 14 |
|
|
| 15 |
TEST: yarp write /writer /icubSim/face/raw/in |
|
| 16 |
|
|
| 17 |
http://wiki.icub.org/wiki/Motor_control |
|
| 18 |
*/ |
|
| 19 |
|
|
| 20 |
#define POSITION_CONTROL 0 |
|
| 5 |
|
|
| 21 | 6 |
using std::cout; |
| 7 |
using std::cerr; |
|
| 8 |
using std::string; |
|
| 22 | 9 |
|
| 23 | 10 |
//! constructor |
| 24 |
iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface(){
|
|
| 11 |
iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface() {
|
|
| 25 | 12 |
scope = _scope; |
| 26 | 13 |
|
| 27 | 14 |
// add mappings from icub ids to humotion ids |
| ... | ... | |
| 34 | 21 |
face_interface_ = new iCubFaceInterface(scope); |
| 35 | 22 |
|
| 36 | 23 |
// intantiate the polydriver |
| 37 |
Property options; |
|
| 24 |
yarp::os::Property options;
|
|
| 38 | 25 |
options.put("device", "remote_controlboard");
|
| 39 | 26 |
options.put("local", "/local/head");
|
| 40 | 27 |
options.put("remote", scope + "/head");
|
| ... | ... | |
| 43 | 30 |
// fetch yarp views: |
| 44 | 31 |
bool success = true; |
| 45 | 32 |
//success &= yarp_polydriver_.view(yarp_iencs_); |
| 46 |
success &= yarp_polydriver_.view(yarp_ipos_); |
|
| 33 |
//success &= yarp_polydriver_.view(yarp_ipos_);
|
|
| 47 | 34 |
success &= yarp_polydriver_.view(yarp_ivel_); |
| 48 | 35 |
success &= yarp_polydriver_.view(yarp_ilimits_); |
| 49 | 36 |
success &= yarp_polydriver_.view(yarp_pid_); |
| ... | ... | |
| 70 | 57 |
void iCubJointInterface::init_controller() {
|
| 71 | 58 |
int number_of_joints; |
| 72 | 59 |
|
| 73 |
//set position mode: |
|
| 74 |
if (POSITION_CONTROL){
|
|
| 75 |
// use position controller, first fetch no of axes: |
|
| 76 |
yarp_ipos_->getAxes(&number_of_joints); |
|
| 77 |
|
|
| 78 |
// set ref acceleration to a value for all axes: |
|
| 79 |
yarp_commands_.resize(number_of_joints); |
|
| 80 |
yarp_commands_ = 200000.0; |
|
| 81 |
yarp_ipos_->setRefAccelerations(yarp_commands_.data()); |
|
| 82 |
yarp_ipos_->setPositionMode(); |
|
| 83 |
}else{
|
|
| 84 |
// use position controller, first fetch no of axes: |
|
| 85 |
yarp_ivel_->getAxes(&number_of_joints); |
|
| 86 |
|
|
| 87 |
// set ref acceleration to a value for all axes: |
|
| 88 |
yarp_commands_.resize(number_of_joints); |
|
| 89 |
yarp_commands_=300.0; |
|
| 90 |
yarp_ivel_->setRefAccelerations(yarp_commands_.data()); |
|
| 91 |
yarp_ivel_->setVelocityMode(); |
|
| 92 |
} |
|
| 93 |
|
|
| 60 |
// use position controller, first fetch no of axes: |
|
| 61 |
yarp_ivel_->getAxes(&number_of_joints); |
|
| 94 | 62 |
|
| 63 |
// set ref acceleration to a value for all axes: |
|
| 64 |
yarp_commands_.resize(number_of_joints); |
|
| 65 |
yarp_commands_=300.0; |
|
| 66 |
yarp_ivel_->setRefAccelerations(yarp_commands_.data()); |
|
| 67 |
yarp_ivel_->setVelocityMode(); |
|
| 95 | 68 |
} |
| 96 | 69 |
|
| 97 | 70 |
//! initialise icub joint id to humotion joint id mappings |
| ... | ... | |
| 205 | 178 |
target_velocity_[icub_id] = velocity; |
| 206 | 179 |
} |
| 207 | 180 |
|
| 208 |
//! execute a move in position mode |
|
| 209 |
//! \param id of joint |
|
| 210 |
//! \param angle |
|
| 211 |
void iCubJointInterface::set_target_in_positionmode(int id) {
|
|
| 212 |
assert(POSITION_CONTROL); |
|
| 213 |
assert(id<=ICUB_ID_EYES_VERGENCE); |
|
| 214 |
|
|
| 215 |
// fetch the current target position |
|
| 216 |
float target = target_angle_[id]; |
|
| 217 |
|
|
| 218 |
// execute motion as position control cmd |
|
| 219 |
yarp_ipos_->positionMove(id, target); |
|
| 220 |
} |
|
| 221 |
|
|
| 222 | 181 |
//! execute a move in velocity mode |
| 223 | 182 |
//! \param id of joint |
| 224 | 183 |
//! \param angle |
| 225 | 184 |
void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
|
| 226 |
assert(!POSITION_CONTROL); |
|
| 227 |
|
|
| 228 | 185 |
// fetch humotion id from icub joint id |
| 229 | 186 |
int humotion_id = convert_icub_jointid_to_humotion(icub_id); |
| 230 | 187 |
|
| ... | ... | |
| 283 | 240 |
//! actually execute the scheduled motion commands |
| 284 | 241 |
void iCubJointInterface::execute_motion(){
|
| 285 | 242 |
|
| 286 |
// set up neck and eye motion commands: |
|
| 287 |
if (POSITION_CONTROL){
|
|
| 288 |
//position control |
|
| 289 |
for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
|
|
| 290 |
set_target_in_positionmode(i); |
|
| 291 |
} |
|
| 292 |
}else{
|
|
| 293 |
//velocity control |
|
| 294 |
for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
|
|
| 295 |
set_target_in_velocitymode(i); |
|
| 296 |
} |
|
| 243 |
// set up neck and eye motion commands in velocitymode |
|
| 244 |
for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
|
|
| 245 |
set_target_in_velocitymode(i); |
|
| 297 | 246 |
} |
| 298 |
//printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
|
|
| 299 |
|
|
| 300 | 247 |
|
| 301 | 248 |
// eyelids: unfortuantely the icub has only 1dof for eyelids |
| 302 | 249 |
// therefore we can only use an opening value |
| ... | ... | |
| 327 | 274 |
} |
| 328 | 275 |
|
| 329 | 276 |
|
| 330 |
void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
|
|
| 331 |
int icub_jointid = -1;
|
|
| 332 |
|
|
| 277 |
void iCubJointInterface::set_joint_enable_state(int humotion_id, bool enable) {
|
|
| 278 |
int icub_jointid = convert_humotion_jointid_to_icub(humotion_id);
|
|
| 279 |
/* |
|
| 333 | 280 |
switch(e){
|
| 334 | 281 |
default: |
| 335 | 282 |
break; |
| ... | ... | |
| 360 | 307 |
icub_jointid = ICUB_ID_EYES_VERGENCE; |
| 361 | 308 |
break; |
| 362 | 309 |
} |
| 310 |
*/ |
|
| 363 | 311 |
|
| 364 |
if (icub_jointid != -1) {
|
|
| 312 |
if ((icub_jointid != -1) && (icub_jointid <= ICUB_ID_EYES_VERGENCE)) {
|
|
| 365 | 313 |
if (enable) {
|
| 366 | 314 |
yarp_amp_->enableAmp(icub_jointid); |
| 367 | 315 |
yarp_pid_->enablePid(icub_jointid); |
| ... | ... | |
| 385 | 333 |
set_joint_enable_state(e, false); |
| 386 | 334 |
} |
| 387 | 335 |
|
| 388 |
void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
|
|
| 336 |
void iCubJointInterface::store_min_max(yarp::dev::IControlLimits *ilimits, int icub_id){
|
|
| 389 | 337 |
double min, max; |
| 390 |
ilimits->getLimits(id, &min, &max); |
|
| 391 |
joint_min[e] = min; |
|
| 392 |
joint_max[e] = max; |
|
| 338 |
int humotion_id = convert_icub_jointid_to_humotion(icub_id); |
|
| 339 |
|
|
| 340 |
ilimits->getLimits(icub_id, &min, &max); |
|
| 341 |
joint_min[humotion_id] = min; |
|
| 342 |
joint_max[humotion_id] = max; |
|
| 393 | 343 |
} |
| 394 | 344 |
|
| 395 | 345 |
//! initialise a joint (set up controller mode etc) |
| 396 |
//! \param joint enum |
|
| 397 | 346 |
void iCubJointInterface::init_joints(){
|
| 398 |
store_min_max(yarp_ilimits_, ICUB_ID_NECK_TILT, ID_NECK_TILT);
|
|
| 399 |
store_min_max(yarp_ilimits_, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);
|
|
| 400 |
store_min_max(yarp_ilimits_, ICUB_ID_NECK_PAN, ID_NECK_PAN);
|
|
| 401 |
store_min_max(yarp_ilimits_, ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD);
|
|
| 347 |
store_min_max(yarp_ilimits_, ICUB_ID_NECK_TILT); |
|
| 348 |
store_min_max(yarp_ilimits_, ICUB_ID_NECK_ROLL); |
|
| 349 |
store_min_max(yarp_ilimits_, ICUB_ID_NECK_PAN); |
|
| 350 |
store_min_max(yarp_ilimits_, ICUB_ID_EYES_BOTH_UD); |
|
| 402 | 351 |
|
| 403 | 352 |
//icub handles eyes differently, we have to set pan angle + vergence |
| 404 | 353 |
double pan_min, pan_max, vergence_min, vergence_max; |
| ... | ... | |
| 444 | 393 |
enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(humotion_id); |
| 445 | 394 |
if(it == enum_id_bimap.right.end()) {
|
| 446 | 395 |
//key does not exist |
| 447 |
cout << "ERROR: invalid humotion joint id (" << humotion_id << ") given. can not convert this. exiting\n";
|
|
| 396 |
cerr << "ERROR: invalid humotion joint id (" << humotion_id << ") given. can not convert this. exiting\n";
|
|
| 448 | 397 |
exit(EXIT_FAILURE); |
| 449 | 398 |
} |
| 450 | 399 |
return it->second; |
Also available in: Unified diff