Revision d3aa8ab3 examples/yarp_icub/src/icub_jointinterface.cpp
| examples/yarp_icub/src/icub_jointinterface.cpp | ||
|---|---|---|
| 168 | 168 |
//! \param id of joint |
| 169 | 169 |
//! \param float value of position |
| 170 | 170 |
void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {
|
| 171 |
printf("> set icub joint %d -> p = %f, v = %f\n",icub_id,position,velocity);
|
|
| 171 |
cout << "store_icub_joint_target(" << icub_id << ", " << position << ", ..)\n";
|
|
| 172 |
|
|
| 172 | 173 |
if (icub_id == ICUB_ID_NECK_PAN) {
|
| 173 | 174 |
// icub uses an inverted neck pan specification |
| 174 | 175 |
position = -position; |
| 175 | 176 |
velocity = -velocity; |
| 176 | 177 |
} |
| 178 |
|
|
| 179 |
// store values |
|
| 177 | 180 |
target_angle_[icub_id] = position; |
| 178 | 181 |
target_velocity_[icub_id] = velocity; |
| 179 | 182 |
} |
| ... | ... | |
| 223 | 226 |
+ pv_mix_pid_p_[icub_id]*error_d |
| 224 | 227 |
+ target_velocity; |
| 225 | 228 |
|
| 226 |
printf("%f %f %f %f %f %f PID%d\n",
|
|
| 227 |
get_ts_position(humotion_id).get_interpolated_value(data_ts), |
|
| 228 |
target_angle_[icub_id], |
|
| 229 |
123.4, //NOT USED ANYMORE//get_ts_speed(humotion_id).get_interpolated_value(data_ts), |
|
| 230 |
pv_mix_velocity, |
|
| 231 |
target_velocity, |
|
| 232 |
position_error, |
|
| 233 |
icub_id |
|
| 234 |
); |
|
| 229 |
cout << "\n" |
|
| 230 |
<< get_ts_position(humotion_id).get_interpolated_value(data_ts) << " " |
|
| 231 |
<< target_angle_[icub_id] << " " |
|
| 232 |
<< 123.4 << " " |
|
| 233 |
<< pv_mix_velocity << " " |
|
| 234 |
<< target_velocity << " " |
|
| 235 |
<< position_error << " " |
|
| 236 |
<< icub_id << " PID\n"; |
|
| 235 | 237 |
|
| 236 | 238 |
// execute velocity move |
| 237 | 239 |
yarp_ivel_->velocityMove(icub_id, pv_mix_velocity); |
Also available in: Unified diff