Revision 18e9b892 examples/yarp_icub/src/icub_data_receiver.cpp
| examples/yarp_icub/src/icub_data_receiver.cpp | ||
|---|---|---|
| 84 | 84 |
|
| 85 | 85 |
#if ICUB_DATA_RECEIVER_USE_ENCODERSPEED |
| 86 | 86 |
// fetch data from icub. NOTE: make sure to enable the vel broadcast in the ini file! |
| 87 |
iencs_->getEncoderSpeeds(velocities_.data()); |
|
| 87 |
if (! iencs_->getEncoderSpeeds(velocities_.data())){
|
|
| 88 |
cout << "Failed to fetch encoder speeds...\n"; |
|
| 89 |
return; |
|
| 90 |
} |
|
| 88 | 91 |
#else |
| 89 | 92 |
// manually calculate the speed based on old position: |
| 90 | 93 |
velocities_ = calculate_velocities(positions_, timestamps_); |
| ... | ... | |
| 126 | 129 |
target_eye_vergence_ = -position; |
| 127 | 130 |
} |
| 128 | 131 |
|
| 129 |
float left = target_eye_pan_ + target_eye_vergence_/2.0;
|
|
| 130 |
float right = target_eye_pan_ - target_eye_vergence_/2.0;
|
|
| 132 |
float right = target_eye_pan_ + target_eye_vergence_/2.0;
|
|
| 133 |
float left = target_eye_pan_ - target_eye_vergence_/2.0;
|
|
| 131 | 134 |
|
| 132 | 135 |
icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_LEFT_LR, |
| 133 | 136 |
left, timestamp); |
| ... | ... | |
| 168 | 171 |
target_eye_vergence_velocity_ = -velocity; |
| 169 | 172 |
} |
| 170 | 173 |
|
| 171 |
float left = target_eye_pan_velocity_ + target_eye_vergence_velocity_/2.0;
|
|
| 172 |
float right = target_eye_pan_velocity_ - target_eye_vergence_velocity_/2.0;
|
|
| 174 |
float right = target_eye_pan_velocity_ + target_eye_vergence_velocity_/2.0;
|
|
| 175 |
float left = target_eye_pan_velocity_ - target_eye_vergence_velocity_/2.0;
|
|
| 173 | 176 |
|
| 174 | 177 |
icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_LEFT_LR, |
| 175 | 178 |
left, timestamp); |
Also available in: Unified diff