humotion

* Excellence Initiative.

*/

#include <humotion/server/joint_interface.h>

#include <humotion/server/server.h>

    void run();

 private:

    yarp::sig::Vector calculate_velocities(yarp::sig::Vector positions,

                              yarp::sig::Vector timestamps);

    yarp::dev::IEncodersTimed *iencs_;

};

* Forschungsgemeinschaft (DFG) in the context of the German

* Excellence Initiative.

*/

#include "humotion_yarp_icub/icub_jointinterface.h"

    yarp::os::BufferedPort<yarp::os::Bottle> emotion_port;

};

* Excellence Initiative.

*/

#include "humotion_yarp_icub/icub_data_receiver.h"

    enum_id_bimap_t enum_id_bimap;

};

using std::cout;

using std::cerr;

using std::string;

using humotion::server::JointInterface;

using yarp::dev::IEncodersTimed;

using yarp::sig::Vector;

//! main loop routine, called by yarp rate thread

void iCubDataReceiver::run() {

    float velocity;

    // grab pos+vel data:

    iencs_->getEncodersTimed(positions_.data(), timestamps_.data());

    // publish data to humotion

    for (int i = 0; i < positions_.size(); i++) {

        // store position values

        // store velocity

    }

    // small hack to tell humotion to update the lid angle

    // fixme: use real id

    #if ICUB_DATA_RECEIVER_DUMP_DATA

    dump_incoming_data();

//! \param icub _id icub joint id

//! \param position

//! \param timestamp

    // cout << "store_incoming_position(icub=" << icub_id << ", " << position << ")\n";

    // store joint position in humotion backend

        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        // icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

//! \param icub_id icub joint id

//! \param velocity

//! \param timestamp

    // cout << "store_incoming_velocity(icub=" << icub_id << ", " << velocity << ")\n";

    // store joint position in humotion backend

        icub_jointinterface_->store_incoming_velocity(JointInterface::ID_EYES_RIGHT_LR,

                                                     right, timestamp);

    } else if (icub_id == 100) {

        // icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,

        //                                             lid_angle, timestamp);

    } else {

//! \param id {0=left, 1=right)

//! \param angle in degrees

void iCubFaceInterface::set_eyebrow_angle(int id, float *target_angle) {

    if (emotion_port.getOutputCount() > 0) {

        double angle = target_angle[id];

        int icub_val = 0;

//! \param id of joint

//! \param float value of position

void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {

    if ((icub_id == ICUB_ID_NECK_PAN) || (icub_id == ICUB_ID_EYES_VERGENCE)) {

        // icub uses an inverted neck pan specification

    // send it to icub face if

    face_interface_->set_eyelid_angle(opening);

    face_interface_->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle_);

    face_interface_->set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW, target_angle_);

    face_interface_->set_mouth(target_angle_);

    // store joint values which we do not handle on icub here:

    JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,

                                            target_angle_[ICUB_ID_LIP_LEFT_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,