Revision 87b50988 examples/yarp_icub/src/icub_jointinterface.cpp
| examples/yarp_icub/src/icub_jointinterface.cpp | ||
|---|---|---|
| 60 | 60 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER)); |
| 61 | 61 |
enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW)); |
| 62 | 62 |
|
| 63 |
//init pd control variables |
|
| 64 |
enum_id_bimap_t::right_const_iterator it; |
|
| 65 |
for(it = enum_id_bimap.left.begin(); it == enum_id_bimap.left.end(); it++) {
|
|
| 66 |
last_position_error[*it] = 0.0; |
|
| 67 |
PID_P[id] = 0.5; |
|
| 68 |
PID_D[id] = 0.1; |
|
| 69 |
} |
|
| 70 |
|
|
| 71 |
|
|
| 63 | 72 |
Property options; |
| 64 | 73 |
options.put("device", "remote_controlboard");
|
| 65 | 74 |
options.put("local", "/local/head");
|
| ... | ... | |
| 179 | 188 |
//! execute a move in position mode |
| 180 | 189 |
//! \param id of joint |
| 181 | 190 |
//! \param angle |
| 182 |
void iCubJointInterface::set_target_in_positionmode(int id, double value){
|
|
| 191 |
void iCubJointInterface::set_target_in_positionmode(int id){
|
|
| 192 |
double target = target_angle[id]; |
|
| 193 |
|
|
| 183 | 194 |
if (id>ICUB_ID_EYES_VERGENCE){
|
| 184 |
printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
|
|
| 195 |
printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,target);
|
|
| 185 | 196 |
return; |
| 186 | 197 |
} |
| 187 | 198 |
|
| 188 | 199 |
// execute motion as position control cmd |
| 189 |
ipos->positionMove(id, value);
|
|
| 200 |
ipos->positionMove(id, target);
|
|
| 190 | 201 |
|
| 191 | 202 |
} |
| 192 | 203 |
|
| 193 | 204 |
//! execute a move in velocity mode |
| 194 | 205 |
//! \param id of joint |
| 195 | 206 |
//! \param angle |
| 196 |
void iCubJointInterface::set_target_in_velocitymode(int id, double value){
|
|
| 207 |
void iCubJointInterface::set_target_in_velocitymode(int id){
|
|
| 197 | 208 |
// set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?! |
| 198 | 209 |
//first: calculate necessary speed to reach the given target within the next clock tick: |
| 199 | 210 |
double distance = value - target_angle_previous[id]; |
| ... | ... | |
| 225 | 236 |
//execute: |
| 226 | 237 |
//ivel->velocityMove(id, speed); |
| 227 | 238 |
if ((id == ICUB_ID_NECK_PAN) || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) ){
|
| 239 |
//do a pd control for velocity moves that incorporates position errors: |
|
| 240 |
humotion::Timestamp data_ts = get_ts_position(id).get_last_timestamp(); |
|
| 241 |
//TODO: add interpolation into future! |
|
| 242 |
//humotion::Timestamp data_ts = humotion::Timestamp::now(); and extend get_interpol value with get_future_value |
|
| 243 |
double position_error = target_angle[id] - get_ts_position(id).get_interpolated_value(data_ts); |
|
| 244 |
double error_d = (position_error - last_position_error[id]) / (((double) rate)*1000.0); |
|
| 245 |
last_position_error[id] = position_error; |
|
| 246 |
//finally do a PD loop to get the target velocity |
|
| 247 |
double target_velocity = PID_P[id] * position_error + PID_D[id]*error_d + speed; |
|
| 248 |
|
|
| 228 | 249 |
//if (id == ICUB_ID_NECK_PAN) speed = -speed; |
| 229 |
ivel->velocityMove(id, speed);
|
|
| 250 |
ivel->velocityMove(id, target_velocity);
|
|
| 230 | 251 |
printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
|
| 231 | 252 |
} |
| 232 | 253 |
|
Also available in: Unified diff