humotion

/*

* This file is part of humotion

*

* Copyright(c) sschulz <AT> techfak.uni-bielefeld.de

* http://opensource.cit-ec.de/projects/humotion

*

* This file may be licensed under the terms of the

* GNU Lesser General Public License Version 3 (the ``LGPL''),

* or (at your option) any later version.

*

* Software distributed under the License is distributed

* on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either

* express or implied. See the LGPL for the specific language

* governing rights and limitations.

*

* You should have received a copy of the LGPL along with this

* program. If not, go to http://www.gnu.org/licenses/lgpl.html

* or write to the Free Software Foundation, Inc.,

* 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.

*

* The development of this software was supported by the

* Excellence Cluster EXC 277 Cognitive Interaction Technology.

* The Excellence Cluster EXC 277 is a grant of the Deutsche

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*/

#include "server/eye_motion_generator.h"

using namespace std;

using namespace humotion;

using namespace humotion::server;

//! constructor

ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, int dof, float t) : MotionGenerator(j){

    dof_count = dof;

    //create Reflexxes API for <dof> DOF actuator

    reflexxes_api = new ReflexxesAPI(dof, t);

    reflexxes_position_input  = new RMLPositionInputParameters(dof);

    reflexxes_position_output = new RMLPositionOutputParameters(dof);

    //synchronize phase

    reflexxes_motion_flags.SynchronizationBehavior = RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE;

}

//! destructor

ReflexxesMotionGenerator::~ReflexxesMotionGenerator(){

}

//! feed motion generator with target data:

//! \param dof id

//! \param target angle

//! \param max_speed max reachable speed during accel

//! \param max_accel max allowable acceleration

void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target,

                                                   float current_position, float current_speed,

                                                   humotion::Timestamp timestamp,

                                                   float max_speed, float max_accel){

    assert(dof < dof_count);

    //set up reflexxes:

    reflexxes_position_input->TargetPositionVector->VecData[dof]  = target;

    reflexxes_position_input->SelectionVector->VecData[dof]       = true;

    reflexxes_position_input->MaxVelocityVector->VecData[dof]     = max_speed;

    reflexxes_position_input->MaxAccelerationVector->VecData[dof] = max_accel;

    reflexxes_position_input->TargetVelocityVector->VecData[dof]  = 0.0; //target speed is zero (really?)

    // feed back current pos & velocity

    // as we have to deal with some latency we will forecast the current

    // position using the old speed, position and the latency:

    float time_diff = humotion::Timestamp::now().to_seconds() - timestamp.to_seconds();

    if (time_diff > 0.1) {

        printf("WARNING: timestamp of position data older than 0.1s (measured %fs)!\n",time_diff);

        printf("         this should never happen! clocks not in sync?\n");

    }

    // assuming this time difference is small we will now do a linear

    // approximation of the current position based on these old measurements

    // under the assumption that the speed is "constant" during this short period

    float position_now = current_position + time_diff * current_speed;

    printf("HTS: diff = %f ms, pos=%f --> %f\n", time_diff*1000.0,current_position, position_now);

    //reflexxes_position_input->CurrentPositionVector->VecData[dof]  = position_now;

    //reflexxes_position_input->CurrentVelocityVector->VecData[dof]  = current_speed;

    // safety: libreflexxes does not like zero accellerations...

    if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0){

            reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001;

    }

}

//! calculate motion profile

void ReflexxesMotionGenerator::reflexxes_calculate_profile(){

    int res = reflexxes_api->RMLPosition(*reflexxes_position_input, reflexxes_position_output, reflexxes_motion_flags);

    if (res < 0){

        if (res == ReflexxesAPI::RML_ERROR_INVALID_INPUT_VALUES){

            printf("> ReflexxesMotionGenerator --> ReflexxesAPI::RML_ERROR_INVALID_INPUT_VALUES error\n");

        }else{

            printf("> ReflexxesMotionGenerator --> ReflexxesAPI::UNKNOWN_ERROR: reflexxes error %d\n",res);

        }

    }

    //feed back values:

    for(int i=0; i<dof_count; i++){

        reflexxes_position_input->CurrentPositionVector->VecData[i]     = reflexxes_position_output->NewPositionVector->VecData[i];

        reflexxes_position_input->CurrentVelocityVector->VecData[i]     = reflexxes_position_output->NewVelocityVector->VecData[i];

        reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i];

    }

}