humotion

#ifndef INCLUDE_HUMOTION_SERVER_CONTROLLER_H_

#define INCLUDE_HUMOTION_SERVER_CONTROLLER_H_

#include <vector>

#include "humotion/gaze_state.h"

    // check if this timestamp allows a valid conversion:

    Timestamp history_begin =

            joint_interface_->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();

    // printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(),

    // history_begin.to_seconds(), history_end.to_seconds());

        // therefore we will use the last known targets (see below)

        // in case we did not see this timestamp before, show a warning:

        if (last_known_absolute_timestamp_ != relative_target_timestamp) {

            last_known_absolute_target_pan_ = 0.0;

            last_known_absolute_target_tilt_ = 0.0;

            last_known_absolute_target_roll_ = 0.0;

    // printf("pan  now = %4.1f, rel=%4.1f ===> %4.2f\n", pan, relative.pan, absolute_gaze.pan);

    // printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n", tilt, relative.tilt, absolute_gaze.tilt);

    // store debug data:

    // this is the position we had at the ts of the relative target

    // this is the relative movement that was requested

    store_debug_data("controller/pan_target_relative", relative.pan);

    // this is the calculated overall target

    // following atomic instructions:

    mutex::scoped_lock sl(joint_ts_position_map_access_mutex_);

    joint_ts_position_map_[joint_id].insert(timestamp, position);

    incoming_position_count_++;

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

    float neck_pan_target  = requested_neck_state_.pan  + requested_gaze_state_.pan_offset;

    float neck_tilt_target = requested_neck_state_.tilt + requested_gaze_state_.tilt_offset;

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

    float neck_roll_target = requested_gaze_state_.roll + requested_gaze_state_.roll_offset;

    // add breath wave to tilt

    neck_tilt_target += get_breath_offset();

    // pass parameters to reflexxes api

    setup_neckmotion(0, neck_pan_target,  neck_pan_now,  neck_pan_speed,  neck_pan_ts);

    setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed, neck_tilt_ts);

    setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed, neck_roll_ts);

    // call reflexxes to handle profile calculation

    reflexxes_calculate_profile();

                                reflexxes_position_output->NewPositionVector->VecData[2],

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

    /*printf("\n%f %f %f %f %f DBG\n",

            neck_pan_now, neck_pan_target,

            reflexxes_position_output->NewPositionVector->VecData[0],

            reflexxes_position_output->NewVelocityVector->VecData[0]

            );*/

    // get distance to target

    float distance_abs = fabs(target - current_position);

    // get max speed: according to the equation Hmax from [guitton87] there is a linear relation

    // between distance_abs and v_max_head:

    // v_max = 4.39 * d_total + 106.0 (in degrees)

    max_velocity = fmin(max_velocity, config->limit_velocity_neck);

    // max accel: assuming linear acceleration we have:

    /* v ^  _

    *   |  / \

    *   | /   \

    // d_total = a * 2 * d_total / (v_max^2)

    // and therefore

    // a = v_max^2 / d_total

    if (distance_abs > 0.0) {

        max_accel = pow(max_velocity, 2) / distance_abs;

    max_accel = fmin(max_accel, config->limit_acceleration_neck);

    // printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);

    // feed reflexxes api with data

    reflexxes_set_input(dof, target, current_position, current_velocity,

    // synchronize phase

    reflexxes_motion_flags.SynchronizationBehavior =

            RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE;

}

TimestampedList::TimestampedList(unsigned int s) {

    // initialize the list to its desired size:

    tsf_list_.resize(s, now);

}