Revision 6c028e11 examples/yarp_icub/src/icub_data_receiver.cpp
| examples/yarp_icub/src/icub_data_receiver.cpp | ||
|---|---|---|
| 57 | 57 |
} |
| 58 | 58 |
|
| 59 | 59 |
void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {
|
| 60 |
cout << "iCubDataReceiver::store_incoming_position(" << icub_id << ", " << value << "..)\n";
|
|
| 60 |
cout << "iCubDataReceiver::store_incoming_position(icubid=" << icub_id << ", " << value << "..)\n"; |
|
| 61 |
|
|
| 61 | 62 |
// store joint position in humotion backend |
| 62 | 63 |
if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) || |
| 63 | 64 |
(icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {
|
| ... | ... | |
| 67 | 68 |
if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {
|
| 68 | 69 |
target_eye_pan_ = value; |
| 69 | 70 |
} else {
|
| 70 |
target_eye_vergence_ = value; |
|
| 71 |
target_eye_vergence_ = -value;
|
|
| 71 | 72 |
} |
| 72 | 73 |
|
| 73 | 74 |
float left = target_eye_pan_ + target_eye_vergence_/2.0; |
| ... | ... | |
| 82 | 83 |
//icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER, |
| 83 | 84 |
// lid_angle, timestamp); |
| 84 | 85 |
} else {
|
| 86 |
if (icub_id == iCubJointInterface::ID_NECK_PAN) {
|
|
| 87 |
// icub uses an inverted neck pan specification |
|
| 88 |
value = -value; |
|
| 89 |
} |
|
| 90 |
|
|
| 85 | 91 |
// known configured mapping between joint ids |
| 86 | 92 |
int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id); |
| 87 | 93 |
icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp); |
Also available in: Unified diff