humotion

find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs message_generation genmsg)

IF (NOT catkin_FOUND)

  MESSAGE(FATAL_ERROR "Error: could not find ROS middleware!")

INCLUDE(FindPkgConfig)

##libreflexxes

  PKG_CHECK_MODULES(REFLEXXES REQUIRED libReflexxesTypeII>=1.2.3)

  IF (NOT REFLEXXES_FOUND)

    message(FATAL_ERROR "Error: could not find lib libReflexxesTypeII")

  ENDIF ()

SET(REFLEXXES_LIBRARY_DIRS "${REFLEXXES_PREFIX}/lib")

## Specify additional locations of header files

## Your package locations should be listed before other locations

include_directories(BEFORE include)

include_directories(BEFORE include/humotion)

#make sure to use ros messages from current build

<?xml version="1.0" encoding="UTF-8"?>

<!DOCTYPE QtCreatorProject>

<qtcreator>

 <data>

  <variable>ProjectExplorer.Project.ActiveTarget</variable>

  <valuemap type="QVariantMap">

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>

   <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>

   <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>

   <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>

   <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>

   <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">

    <value type="bool" key="CMakeProjectManager.CMakeBuildConfiguration.UseNinja">false</value>

    <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">

     <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">

      <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>

    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.Arguments"></value>

    <value type="bool" key="CMakeProjectManager.CMakeRunConfiguration.UseTerminal">false</value>

    <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory"></value>

    <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">icub_humotion_server</value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>

    <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.icub_humotion_server</value>

    <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>

    <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>

    <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>

    <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Updater.EnvironmentId</variable>

 </data>

 <data>

  <variable>ProjectExplorer.Project.Updater.FileVersion</variable>

    JointInterface();

    ~JointInterface();

    float get_target_position(int joint_id);

    virtual void publish_target_position(int joint_id) = 0;

    virtual void execute_motion() = 0;

    float joint_min[JOINT_ID_ENUM_SIZE];

    float joint_max[JOINT_ID_ENUM_SIZE];

    float joint_target[JOINT_ID_ENUM_SIZE];

    ~ReflexxesMotionGenerator();

protected:

    void reflexxes_calculate_profile();

    //*****************************************

    float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);

    //feed reflexxes api with data

}

//! calculate joint targets

    setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now);

    setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now);

    setup_eyemotion(2, eye_tilt_target, eye_tilt_now);

    //call reflexxes to handle profile calculation:

    reflexxes_calculate_profile();

    //tell the joint about the new values:

}

    float eyebrow_right_target = requested_gaze_state.eyebrow_right;

    //store targets:

}

//! publish targets to motor boards:

    eyelid_lower_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_LOWER,  eyelid_lower_right_target);

    //(temporarily) store the target

    //check for saccade

    check_for_saccade();

        //use the upper value + 10 deg as close state:

        value = joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LID_UPPER) + 10.0;

        //overwrite last target_

        break;

    case(JointInterface::ID_EYES_RIGHT_LID_UPPER):

        //use the upper value + 10 deg as close state:

        value = joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LID_UPPER) + 10.0;

        //overwrite last target_

        break;

    }

}

//! set joint target position

//! \param joint_id of joint

//! \param float position

    assert(joint_id < JOINT_ID_ENUM_SIZE);

    //update current value

    joint_target[joint_id] = position;

}

//! fetch target position

//! \param joint_id

//! \return float value of joint position

float MotionGenerator::get_current_position(int joint_id){

    return joint_interface->get_ts_position(joint_id).get_newest_value();

}

    }

    //tell the joint about the new values:

    //printf("> update_mouth_target(u=%d, l=%d) -> upper = %4.2f, lower = %4.2f\n",upper_id, lower_id,unsafe_target_upper,unsafe_target_lower);

}

    reflexxes_calculate_profile();

    //tell the joint if about the new values:

}

//! publish targets to motor boards:

    if (max_accel>1000){

        max_accel = 1000;

    }

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

    //feed reflexxes api with data

}

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

    dof_count = dof;

    reflexxes_api = new ReflexxesAPI(dof, t);

    reflexxes_position_input  = new RMLPositionInputParameters(dof);

    reflexxes_position_output = new RMLPositionOutputParameters(dof);

//! \param target angle

//! \param max_speed max reachable speed during accel

//! \param max_accel max allowable acceleration

    //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->TargetVelocityVector->VecData[dof]  = 0.0; //target speed is zero (really?)

}

//! calculate motion profile

    //feed back values:

    for(int i=0; i<dof_count; 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];

    }

    }

    //we reached the end, return the last value

    printf("           this should not happen as images will always be behind\n");

    printf("           the motor data. returning most recent value (ts=%f)\n", previous.timestamp.to_seconds());