babbling.h

Go to the documentation of this file.

/* 
 * Copyright (C) 2014 WYSIWYD Consortium, European Commission FP7 Project ICT-612139
 * Authors: Martina Zambelli
 * email:   m.zambelli13@imperial.ac.uk
 * Permission is granted to copy, distribute, and/or modify this program
 * under the terms of the GNU General Public License, version 2 or any
 * later version published by the Free Software Foundation.
 *
 * A copy of the license can be found at
 * icub-client/license/gpl.txt
 *
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
 * Public License for more details
*/

#ifndef _BABBLING_H_
#define _BABBLING_H_

#include <yarp/sig/all.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>

class Babbling : public yarp::os::RFModule {
protected:
    yarp::os::RpcServer handlerPort;

    yarp::dev::IPositionControl* posLeftArm;
    yarp::dev::IVelocityControl* velLeftArm;
    yarp::dev::ITorqueControl *itrqLeftArm;
    yarp::dev::IEncoders* encsLeftArm;
    yarp::dev::IControlMode *ictrlLeftArm;
    yarp::dev::IControlLimits *ictrlLimLeftArm;

    yarp::dev::IPositionControl* posRightArm;
    yarp::dev::IVelocityControl* velRightArm;
    yarp::dev::ITorqueControl *itrqRightArm;
    yarp::dev::IEncoders* encsRightArm;
    yarp::dev::IControlMode *ictrlRightArm;
    yarp::dev::IControlLimits *ictrlLimRightArm;

    yarp::dev::IGazeControl* igaze;

    yarp::dev::PolyDriver leftArmDev;
    yarp::dev::PolyDriver rightArmDev;
    yarp::dev::PolyDriver headDev;

    yarp::sig::VectorOf<double> encodersLeftArm, encodersRightArm;

    std::string part; 
    std::string robot; 
    std::string part_babbling; 
    int single_joint; 

    double freq; 
    double amp; 
    double duration; 
    
    double start_command_arm[16]; 
    yarp::sig::VectorOf<double> start_command_head; 

public:
    bool configure(yarp::os::ResourceFinder &rf);
    bool interruptModule();
    bool close();
    bool respond(const yarp::os::Bottle& command, yarp::os::Bottle& reply);
    double getPeriod();
    bool updateModule();

protected:
    bool init_iCub();
    bool init_left_arm(); 
    bool init_right_arm(); 

    bool doBabbling();


    void babblingCommands(double &t, int j_idx);

    bool moveHeadToStartPos();

    bool gotoStartPos();
};

#endif // _BABBLING_H_