Revision 87b50988
| examples/yarp_icub/CMakeLists.txt.user | ||
|---|---|---|
| 1 | 1 |
<?xml version="1.0" encoding="UTF-8"?> |
| 2 | 2 |
<!DOCTYPE QtCreatorProject> |
| 3 |
<!-- Written by QtCreator 3.0.1, 2016-02-04T09:55:06. -->
|
|
| 3 |
<!-- Written by QtCreator 3.0.1, 2016-02-09T13:32:45. -->
|
|
| 4 | 4 |
<qtcreator> |
| 5 | 5 |
<data> |
| 6 | 6 |
<variable>ProjectExplorer.Project.ActiveTarget</variable> |
| examples/yarp_icub/include/icub_jointinterface.h | ||
|---|---|---|
| 86 | 86 |
yarp::dev::IAmplifierControl *amp; |
| 87 | 87 |
yarp::dev::IPidControl *pid; |
| 88 | 88 |
yarp::dev::IControlMode *icontrol; |
| 89 |
std::vector<double> last_position_error; |
|
| 90 |
std::vector<double> PID_P; |
|
| 91 |
std::vector<double> PID_D; |
|
| 89 | 92 |
|
| 90 | 93 |
void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e); |
| 91 | 94 |
|
| ... | ... | |
| 95 | 98 |
float last_vel_eye_pan; |
| 96 | 99 |
|
| 97 | 100 |
void store_joint(int id, float value); |
| 98 |
void set_target_in_positionmode(int id, double value);
|
|
| 99 |
void set_target_in_velocitymode(int id, double value);
|
|
| 101 |
void set_target_in_positionmode(int id); |
|
| 102 |
void set_target_in_velocitymode(int id); |
|
| 100 | 103 |
|
| 101 | 104 |
|
| 102 | 105 |
int convert_enum_to_motorid(int e); |
| 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 |
|
| src/server/reflexxes_motion_generator.cpp | ||
|---|---|---|
| 83 | 83 |
printf("HTS: diff = %f ms, pos=%f --> %f\n", time_diff*1000.0,current_position, position_now);
|
| 84 | 84 |
|
| 85 | 85 |
|
| 86 |
reflexxes_position_input->CurrentPositionVector->VecData[dof] = position_now; |
|
| 86 |
//reflexxes_position_input->CurrentPositionVector->VecData[dof] = position_now;
|
|
| 87 | 87 |
//reflexxes_position_input->CurrentVelocityVector->VecData[dof] = current_speed; |
| 88 | 88 |
|
| 89 | 89 |
// safety: libreflexxes does not like zero accellerations... |
| ... | ... | |
| 108 | 108 |
|
| 109 | 109 |
//feed back values: |
| 110 | 110 |
for(int i=0; i<dof_count; i++){
|
| 111 |
//reflexxes_position_input->CurrentPositionVector->VecData[i] = reflexxes_position_output->NewPositionVector->VecData[i];
|
|
| 111 |
reflexxes_position_input->CurrentPositionVector->VecData[i] = reflexxes_position_output->NewPositionVector->VecData[i]; |
|
| 112 | 112 |
reflexxes_position_input->CurrentVelocityVector->VecData[i] = reflexxes_position_output->NewVelocityVector->VecData[i]; |
| 113 | 113 |
reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i]; |
| 114 | 114 |
} |
Also available in: Unified diff