humotion

//! constructor

NeckMotionGenerator::NeckMotionGenerator(JointInterface *j) :

    GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE) {

}

    // -> triangular wave as speeds -> (x<0.5)? 2*x*x:  1- 2*(1-x)**2 = 4x - 2x**2 - 1

    float breath_offset = 0.0;

    // 0...1 -> move up, 1..2 -> return, 2..3 -> still

    if (breath_time_normalized <= 0.5) {

        // accelerated motion

    }

    // fetch next time

    }

    return breath_offset;

                                                            &neck_roll_speed);

    // reached target?

    // printf("GOAL DIFF = %f TARGET DIFF = %f\n",goal_diff,target_diff);

    // get_current_gaze().dump();

    // check if new target

    // close to goal?

    }

    }

    // if we get a new target now, we can stop the neck saccade

    if (target_diff > .1) {

            // joint_interface->neck_saccade_done();

        }

    }

    if (neck_saccade_requested) {

    }

    // check if this is a small or big saccade

        // full saccade with neck motion -> update neck target

    }

    // get targets: this is the sum of stored neck target and up-to-date offset:

    // roll is always equal to requested gaze (not neck) state

    // add breath wave to tilt

    neck_tilt_target += get_breath_offset();

    reflexxes_calculate_profile();

    // tell the joint if about the new values

                                reflexxes_position_output->NewPositionVector->VecData[0],

                                reflexxes_position_output->NewVelocityVector->VecData[0]);

                                reflexxes_position_output->NewPositionVector->VecData[1],

                                reflexxes_position_output->NewVelocityVector->VecData[1]);

                                reflexxes_position_output->NewPositionVector->VecData[2],

                                reflexxes_position_output->NewVelocityVector->VecData[2]);

void NeckMotionGenerator::publish_targets() {

    // publish values if there is an active gaze input within the last timerange

    if (gaze_target_input_active()) {

    }

}