actionPrimitives.h

Go to the documentation of this file.

/* 
 * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
 * Author: Ugo Pattacini
 * email:  ugo.pattacini@iit.it
 * website: www.robotcub.org
 * 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
 * http://www.robotcub.org/icub/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 __AFFACTIONPRIMITIVES_H__
#define __AFFACTIONPRIMITIVES_H__
 
#include <mutex>
#include <condition_variable>
#include <string>
#include <vector>
#include <deque>
#include <set>
#include <map>
 
#include <yarp/os/all.h>
#include <yarp/dev/all.h>
#include <yarp/sig/all.h>
 
#include <iCub/perception/models.h>
 
#define ACTIONPRIM_DISABLE_EXECTIME    -1.0
 
 
namespace iCub
{
 
namespace action
{
 
class ActionPrimitivesCallback
{
public:
    ActionPrimitivesCallback() { }
 
    virtual void exec() = 0;
};
 
 
struct ActionPrimitivesWayPoint
{
    yarp::sig::Vector x;
 
    yarp::sig::Vector o;
 
    bool oEnabled;
 
    double duration;
 
    double trajTime;
 
    double granularity;
 
    ActionPrimitivesCallback *callback;
 
    ActionPrimitivesWayPoint();
};
 
 
class ActionPrimitives : protected yarp::os::PeriodicThread
{
protected:
    std::string robot;
    std::string local;
    std::string part;
 
    yarp::dev::PolyDriver         polyHand;
    yarp::dev::PolyDriver         polyCart;
    yarp::dev::IControlMode      *modCtrl;
    yarp::dev::IEncoders         *encCtrl;
    yarp::dev::IPositionControl  *posCtrl;
    yarp::dev::ICartesianControl *cartCtrl;
 
    perception::Model            *graspModel;
                                 
    yarp::os::PeriodicThread     *armWaver;
    std::mutex                    mtx;
    std::mutex                    mtx_motionStartEvent;
    std::condition_variable       cv_motionStartEvent;
    std::mutex                    mtx_motionDoneEvent;
    std::condition_variable       cv_motionDoneEvent;
 
    bool armMoveDone;
    bool handMoveDone;
    bool latchArmMoveDone;
    bool latchHandMoveDone;
    bool handSeqTerminator;
    bool fingersInPosition;
    std::deque<bool> fingerInPosition;
 
    bool configured;
    bool closed;
    bool checkEnabled;    
    bool tracking_mode;
    bool torsoActive;
    bool reachTmoEnabled;
    bool locked;
    bool verbose;
 
    double default_exec_time;
    double waitTmo;
    double reachTmo;
    double latchTimerWait;
    double latchTimerReach;
    double latchTimerReachLog;
 
    int jHandMin;
    int jHandMax;
 
    yarp::sig::Vector enableTorsoSw;
    yarp::sig::Vector disableTorsoSw;
                      
    yarp::sig::Vector curHandFinalPoss;
    yarp::sig::Vector curHandTols;
    yarp::sig::Vector curGraspDetectionThres;
    double            curHandTmo;
    double            latchTimerHand;
 
    std::vector<int>       fingersJnts;
    std::set<int>          fingersJntsSet;
    std::set<int>          fingersMovingJntsSet;
    std::multimap<int,int> fingers2JntsMap;
 
    friend class ArmWayPoints;
 
    struct HandWayPoint
    {
        std::string       tag;
        yarp::sig::Vector poss;
        yarp::sig::Vector vels;
        yarp::sig::Vector tols;
        yarp::sig::Vector thres;
        double            tmo;
    };
 
    struct Action
    {
        // wait action
        bool waitState;
        double tmo;
        // reach action
        bool execArm;
        yarp::sig::Vector x;
        yarp::sig::Vector o;
        double execTime;
        bool oEnabled;
        // hand action
        bool execHand;
        HandWayPoint handWP;
        bool handSeqTerminator;
        // reach way points action
        bool execWayPoints;
        yarp::os::PeriodicThread *wayPointsThr;
        // action callback
        ActionPrimitivesCallback *clb;
    };
 
    ActionPrimitivesCallback *actionClb;
    yarp::os::PeriodicThread *actionWP;
    class ActionsQueue : public std::deque<Action>
    {
    public:
        void clear();
    } actionsQueue;
    std::map<std::string,std::deque<HandWayPoint> > handSeqMap;
    
    virtual void printMessage(const int logtype, const char *format, ...) const;
 
    virtual bool handleTorsoDOF(yarp::os::Property &opt, const std::string &key);
    virtual void disableTorsoDof();
    virtual void enableTorsoDof();
    virtual bool configHandSeq(yarp::os::Property &opt);
    virtual bool configGraspModel(yarp::os::Property &opt);
    virtual bool _pushAction(const bool execArm, const yarp::sig::Vector &x,
                             const yarp::sig::Vector &o, const double execTime,
                             const bool oEnabled, const bool execHand, const HandWayPoint &handWP,
                             const bool handSeqTerminator, ActionPrimitivesCallback *clb);
    virtual bool _pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                             const std::string &handSeqKey, const double execTime,
                             ActionPrimitivesCallback *clb, const bool oEnabled);
    virtual bool handCheckMotionDone(const int jnt);
    virtual bool wait(const Action &action);
    virtual bool cmdArm(const Action &action);
    virtual bool cmdArmWP(const Action &action);
    virtual bool cmdHand(const Action &action);
    virtual bool isHandSeqEnded();
    virtual void postReachCallback();
 
    virtual void init();
    virtual bool execQueuedAction();
    virtual bool execPendingHandSequences();
    virtual void run();
 
public:
    ActionPrimitives();
 
    ActionPrimitives(yarp::os::Property &opt);
 
    virtual ~ActionPrimitives();
 
    virtual bool open(yarp::os::Property &opt);
 
    virtual bool isValid() const;
 
    virtual void close();
 
    virtual bool pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o, 
                            const std::string &handSeqKey,
                            const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
                            ActionPrimitivesCallback *clb=NULL);
 
    virtual bool pushAction(const yarp::sig::Vector &x, 
                            const std::string &handSeqKey,
                            const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
                            ActionPrimitivesCallback *clb=NULL);
 
    virtual bool pushAction(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                            const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
                            ActionPrimitivesCallback *clb=NULL);
 
    virtual bool pushAction(const yarp::sig::Vector &x,
                            const double execTime=ACTIONPRIM_DISABLE_EXECTIME,
                            ActionPrimitivesCallback *clb=NULL);
 
    virtual bool pushAction(const std::string &handSeqKey,
                            ActionPrimitivesCallback *clb=NULL);
 
    virtual bool pushAction(const std::deque<ActionPrimitivesWayPoint> &wayPoints,
                            ActionPrimitivesCallback *clb=NULL);
 
    virtual bool pushAction(const std::deque<ActionPrimitivesWayPoint> &wayPoints,
                            const std::string &handSeqKey, ActionPrimitivesCallback *clb=NULL);
 
    virtual bool pushWaitState(const double tmo, ActionPrimitivesCallback *clb=NULL);
 
    virtual bool reachPose(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                           const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
 
    virtual bool reachPosition(const yarp::sig::Vector &x,
                               const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
 
    virtual bool clearActionsQueue();
 
    virtual bool lockActions();
 
    virtual bool unlockActions();
 
    virtual bool getActionsLockStatus() const;
 
    virtual bool addHandSeqWP(const std::string &handSeqKey, const yarp::sig::Vector &poss,
                              const yarp::sig::Vector &vels, const yarp::sig::Vector &tols,
                              const yarp::sig::Vector &thres, const double tmo);
 
    virtual bool addHandSequence(const std::string &handSeqKey,
                                 const yarp::os::Bottle &sequence);
 
    virtual bool isValidHandSeq(const std::string &handSeqKey);
 
    virtual bool removeHandSeq(const std::string &handSeqKey);
 
    std::deque<std::string> getHandSeqList();
 
    virtual bool getHandSequence(const std::string &handSeqKey, yarp::os::Bottle &sequence);
 
    virtual bool areFingersMoving(bool &f);
 
    virtual bool areFingersInPosition(bool &f);
 
    virtual bool areFingersInPosition(std::deque<bool> &f);
 
    virtual bool getGraspModel(perception::Model *&model) const;
 
    virtual bool getCartesianIF(yarp::dev::ICartesianControl *&ctrl) const;
 
    virtual bool getTorsoJoints(yarp::sig::Vector &torso);
 
    virtual bool setTorsoJoints(const yarp::sig::Vector &torso);
 
    virtual bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o) const;
 
    virtual bool stopControl();
 
    virtual bool setDefaultExecTime(const double execTime);
 
    virtual double getDefaultExecTime() const;
 
    virtual bool setTrackingMode(const bool f);
 
    virtual bool getTrackingMode() const;
 
    virtual bool enableArmWaving(const yarp::sig::Vector &restPos);
 
    virtual bool disableArmWaving();
 
    virtual bool enableReachingTimeout(const double tmo);
 
    virtual bool disableReachingTimeout();
 
    virtual bool checkActionsDone(bool &f, const bool sync=false);
 
    virtual bool checkActionOnGoing(bool &f, const bool sync=false);
 
    virtual bool syncCheckInterrupt(const bool disable=false);
 
    virtual bool syncCheckReinstate();    
};
 
 
class ActionPrimitivesLayer1 : public ActionPrimitives
{
public:
    ActionPrimitivesLayer1() : ActionPrimitives() { }
 
    ActionPrimitivesLayer1(yarp::os::Property &opt) : ActionPrimitives(opt) { }
 
    virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                       const yarp::sig::Vector &d);
 
    virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                       const yarp::sig::Vector &d);
 
    virtual bool tap(const yarp::sig::Vector &x1, const yarp::sig::Vector &o1,
                     const yarp::sig::Vector &x2, const yarp::sig::Vector &o2,
                     const double execTime=ACTIONPRIM_DISABLE_EXECTIME);
};
 
 
// forward declaration
class ActionPrimitivesLayer2;
 
 
// callback for executing final grasp after contact
class liftAndGraspCallback : public ActionPrimitivesCallback
{
protected:
    ActionPrimitivesLayer2 *action;
 
public:
    liftAndGraspCallback(ActionPrimitivesLayer2 *_action) :
                         action(_action) { }
 
    virtual void exec();
};
 
 
// callback for executing touch callback
class touchCallback : public ActionPrimitivesCallback
{
protected:
    ActionPrimitivesLayer2 *action;
 
public:
    touchCallback(ActionPrimitivesLayer2 *_action) :
                  action(_action) { }
 
    virtual void exec();
};
 
 
class ActionPrimitivesLayer2 : public ActionPrimitivesLayer1
{
protected:
    bool skipFatherPart;
    bool configuredLayer2;
    bool contactDetectionOn;
    bool contactDetected;
 
    double ext_force_thres;
 
    liftAndGraspCallback *execLiftAndGrasp;
    touchCallback        *execTouch;
 
    yarp::os::BufferedPort<yarp::sig::Vector> wbdynPortIn;
 
    yarp::sig::Vector wrenchExternal;
 
    yarp::sig::Vector grasp_d2;
    yarp::sig::Vector grasp_o;
 
    yarp::sig::Vector encDataTorso;
    yarp::sig::Vector encDataArm;
 
    friend class liftAndGraspCallback;
    friend class touchCallback;
 
    virtual void init();
    virtual void postReachCallback();
    virtual void run();
 
public:
    ActionPrimitivesLayer2();
 
    ActionPrimitivesLayer2(yarp::os::Property &opt);
 
    virtual ~ActionPrimitivesLayer2();
 
    virtual bool open(yarp::os::Property &opt);
 
    virtual bool isValid() const;
 
    virtual void close();
 
    virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                       const yarp::sig::Vector &d1, const yarp::sig::Vector &d2);
 
    virtual bool grasp(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                       const yarp::sig::Vector &d);
 
    virtual bool touch(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
                       const yarp::sig::Vector &d);
 
    virtual bool getExtWrench(yarp::sig::Vector &wrench) const;
 
    virtual bool getExtForceThres(double &thres) const;
 
    virtual bool setExtForceThres(const double thres);
 
    virtual bool enableContactDetection();
 
    virtual bool disableContactDetection();
 
    virtual bool isContactDetectionEnabled(bool &f) const;
 
    virtual bool checkContact(bool &f) const;
};
 
}
 
}
 
#endif