Revision 730467d3 src/server/neck_motion_generator.cpp
| src/server/neck_motion_generator.cpp | ||
|---|---|---|
| 91 | 91 |
|
| 92 | 92 |
//! calculate joint targets |
| 93 | 93 |
void NeckMotionGenerator::calculate_targets(){
|
| 94 |
//fetch current angles: |
|
| 95 |
float neck_pan_now = get_current_position(JointInterface::ID_NECK_PAN); |
|
| 96 |
float neck_tilt_now = get_current_position(JointInterface::ID_NECK_TILT); |
|
| 97 |
float neck_roll_now = get_current_position(JointInterface::ID_NECK_ROLL); |
|
| 98 |
// fetch current velocities |
|
| 99 |
float neck_pan_speed = get_current_speed(JointInterface::ID_NECK_PAN); |
|
| 100 |
float neck_tilt_speed = get_current_speed(JointInterface::ID_NECK_TILT); |
|
| 101 |
float neck_roll_speed = get_current_speed(JointInterface::ID_NECK_ROLL); |
|
| 94 |
//fetch current dataset: |
|
| 95 |
float neck_pan_now, neck_tilt_now, neck_roll_now; |
|
| 96 |
float neck_pan_speed, neck_tilt_speed, neck_roll_speed; |
|
| 97 |
|
|
| 98 |
humotion::Timestamp neck_pan_ts = get_timestamped_state(JointInterface::ID_NECK_PAN, |
|
| 99 |
&neck_pan_now, |
|
| 100 |
&neck_pan_speed); |
|
| 101 |
|
|
| 102 |
humotion::Timestamp neck_tilt_ts = get_timestamped_state(JointInterface::ID_NECK_TILT, |
|
| 103 |
&neck_tilt_now, |
|
| 104 |
&neck_tilt_speed); |
|
| 105 |
|
|
| 106 |
humotion::Timestamp neck_roll_ts = get_timestamped_state(JointInterface::ID_NECK_ROLL, |
|
| 107 |
&neck_roll_now, |
|
| 108 |
&neck_roll_speed); |
|
| 102 | 109 |
|
| 103 | 110 |
//reached target? |
| 104 | 111 |
float goal_diff = fabs(get_current_gaze().distance_pt_abs(requested_gaze_state)); |
| ... | ... | |
| 147 | 154 |
neck_tilt_target += get_breath_offset(); |
| 148 | 155 |
|
| 149 | 156 |
//pass parameters to reflexxes api: |
| 150 |
setup_neckmotion(0, neck_pan_target, neck_pan_now, neck_pan_speed);
|
|
| 151 |
setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed); |
|
| 152 |
setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed); |
|
| 157 |
setup_neckmotion(0, neck_pan_target, neck_pan_now, neck_pan_speed, neck_pan_ts);
|
|
| 158 |
setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed, neck_tilt_ts);
|
|
| 159 |
setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed, neck_roll_ts);
|
|
| 153 | 160 |
|
| 154 | 161 |
//call reflexxes to handle profile calculation: |
| 155 | 162 |
reflexxes_calculate_profile(); |
| ... | ... | |
| 167 | 174 |
reflexxes_position_output->NewPositionVector->VecData[2], |
| 168 | 175 |
reflexxes_position_output->NewVelocityVector->VecData[2]); |
| 169 | 176 |
|
| 170 |
printf("\n%f %f %f %f %f %f DBG\n",
|
|
| 177 |
printf("\n%f %f %f %f %f DBG\n",
|
|
| 171 | 178 |
neck_pan_now, neck_pan_target, |
| 172 | 179 |
reflexxes_position_output->NewPositionVector->VecData[0], |
| 173 | 180 |
joint_interface->get_ts_speed(JointInterface::ID_NECK_PAN).get_newest_value(), |
| ... | ... | |
| 191 | 198 |
//! \param dof id of joint |
| 192 | 199 |
//! \param target angle |
| 193 | 200 |
//! \param current angle |
| 194 |
void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position, float current_speed){
|
|
| 201 |
void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position, |
|
| 202 |
float current_speed, humotion::Timestamp timestamp){
|
|
| 195 | 203 |
//get distance to target: |
| 196 | 204 |
float distance_abs = fabs(target - current_position); |
| 197 | 205 |
|
| ... | ... | |
| 228 | 236 |
///printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);
|
| 229 | 237 |
|
| 230 | 238 |
//feed reflexxes api with data |
| 231 |
reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel); |
|
| 239 |
reflexxes_set_input(dof, target, current_position, current_speed, timestamp, max_speed, max_accel);
|
|
| 232 | 240 |
} |
Also available in: Unified diff