iKinSlv.h

Go to the documentation of this file.

/*
 * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
 * Copyright (C) 2006-2010 RobotCub Consortium
 * All rights reserved.
 *
 * This software may be modified and distributed under the terms
 * of the BSD-3-Clause license. See the accompanying LICENSE file for
 * details.
*/
 
#ifndef __IKINSLV_H__
#define __IKINSLV_H__
 
#include <mutex>
#include <condition_variable>
#include <string>
#include <deque>
 
#include <yarp/os/BufferedPort.h>
#include <yarp/os/PeriodicThread.h>
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
 
#include <yarp/dev/ControlBoardInterfaces.h>
#include <yarp/dev/PolyDriver.h>
 
#include <iCub/iKin/iKinHlp.h>
#include <iCub/iKin/iKinIpOpt.h>
 
 
namespace iCub
{
 
namespace iKin
{
 
class CartesianSolver;
 
class RpcProcessor : public yarp::os::PortReader
{
protected:
    CartesianSolver *slv;
 
    virtual bool read(yarp::os::ConnectionReader &connection);
 
public:
    RpcProcessor(CartesianSolver *_slv) : slv(_slv) { }
};
 
 
class InputPort : public yarp::os::BufferedPort<yarp::os::Bottle>
{
protected:
    CartesianSolver *slv;
 
    std::mutex mtx;
 
    bool   contMode;
    bool   isNew;
    size_t maxLen;
    int    pose;
 
    double  token;
    double *pToken;
 
    yarp::sig::Vector dof;
    yarp::sig::Vector xd;
 
    virtual void onRead(yarp::os::Bottle &b);
 
public:
    InputPort(CartesianSolver *_slv);
 
    int               &get_pose()     { return pose;     }
    bool              &get_contMode() { return contMode; }
    double            *get_tokenPtr() { return pToken;   }
    yarp::sig::Vector  get_dof();
    yarp::sig::Vector  get_xd();
    
    void set_dof(const yarp::sig::Vector &_dof);
    void reset_xd(const yarp::sig::Vector &_xd);
    bool isNewDataEvent();
    bool handleTarget(yarp::os::Bottle *b);
    bool handleDOF(yarp::os::Bottle *b);
    bool handlePose(const int newPose);
    bool handleMode(const int newMode);    
};
 
 
class SolverCallback : public iKinIterateCallback
{
protected:
    CartesianSolver *slv;
 
public:
    SolverCallback(CartesianSolver *_slv) : slv(_slv) { }
 
    virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q);
};
 
 
struct PartDescriptor
{
    iKinLimb                      *lmb;
    iKinChain                     *chn;
    iKinLinIneqConstr             *cns;
    std::deque<yarp::os::Property> prp;
    std::deque<bool>               rvs;
    int                            num;
};
 
 
class CartesianSolver : public    yarp::os::PeriodicThread,
                        protected CartesianHelper
{
protected:
    PartDescriptor                        *prt;
    std::deque<yarp::dev::PolyDriver*>     drv;
    std::deque<yarp::dev::IControlLimits*> lim;
    std::deque<yarp::dev::IEncoders*>      enc;
    std::deque<int>                        jnt;
    std::deque<int*>                       rmp;
 
    iKinIpOptMin   *slv;
    SolverCallback *clb;
 
    RpcProcessor                             *cmdProcessor;
    yarp::os::Port                           *rpcPort;
    InputPort                                *inPort;
    yarp::os::BufferedPort<yarp::os::Bottle> *outPort;
    std::mutex                                mtx;
 
    std::string   slvName;
    std::string   type;
    unsigned int  period;
    unsigned int  ctrlPose;
    bool          fullDOF;
    bool          contModeOld;
    bool          configured;
    bool          closing;
    bool          closed;
    bool          interrupting;
    bool          verbosity;
    bool          timeout_detected;
    int           maxPartJoints;
    int           unctrlJointsNum;
    double        ping_robot_tmo;
    double        token;
    double       *pToken;
 
    yarp::sig::Matrix hwLimits;
    yarp::sig::Matrix swLimits;
 
    yarp::sig::Vector unctrlJointsOld;
    yarp::sig::Vector dof;
 
    yarp::sig::Vector restJntPos;
    yarp::sig::Vector restWeights;
 
    yarp::sig::Vector xd_2ndTask;
    yarp::sig::Vector w_2ndTask;
 
    yarp::sig::Vector qd_3rdTask;
    yarp::sig::Vector w_3rdTask;
    yarp::sig::Vector idx_3rdTask;
 
    std::mutex mtx_dofEvent;
    std::condition_variable cv_dofEvent;
 
    virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options)=0;
    virtual yarp::sig::Vector solve(yarp::sig::Vector &xd);
 
    virtual yarp::sig::Vector &encodeDOF();
    virtual bool decodeDOF(const yarp::sig::Vector &_dof);
 
    virtual bool handleJointsRestPosition(const yarp::os::Bottle *options,
                                          yarp::os::Bottle *reply=NULL);
    virtual bool handleJointsRestWeights(const yarp::os::Bottle *options,
                                         yarp::os::Bottle *reply=NULL);
 
    yarp::dev::PolyDriver *waitPart(const yarp::os::Property &partOpt);
    
    bool isNewDOF(const yarp::sig::Vector &_dof);
    bool changeDOF(const yarp::sig::Vector &_dof);
 
    bool alignJointsBounds();
    bool setLimits(int axis, double min, double max);
    void countUncontrolledJoints();
    void latchUncontrolledJoints(yarp::sig::Vector &joints);
    void getFeedback(const bool wait=false);    
    void initPos();
    void lock();
    void unlock();    
 
    void waitDOFHandling();
    void postDOFHandling();
    void fillDOFInfo(yarp::os::Bottle &reply);
    void send(const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok);
    void printInfo(const std::string &typ, const yarp::sig::Vector &xd, const yarp::sig::Vector &x,
                   const yarp::sig::Vector &q, const double t);    
 
    virtual void prepareJointsRestTask();
    virtual void respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply);
    virtual bool threadInit();
    virtual void afterStart(bool);
    virtual void run();
    virtual void threadRelease();
 
    friend class RpcProcessor;
    friend class InputPort;
    friend class SolverCallback;
 
public:
    CartesianSolver(const std::string &_slvName);
 
    virtual bool open(yarp::os::Searchable &options);
 
    virtual void interrupt();
 
    virtual void close();
 
    virtual bool isClosed() const { return closed; }
 
    virtual bool &getTimeoutFlag() { return timeout_detected; }
 
    virtual void suspend();
 
    virtual void resume();
 
    virtual ~CartesianSolver();
};
 
 
class iCubArmCartesianSolver : public CartesianSolver
{
protected:
    virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
    virtual bool decodeDOF(const yarp::sig::Vector &_dof);
 
public:
    iCubArmCartesianSolver(const std::string &_slvName="armCartSolver") : CartesianSolver(_slvName) { }
 
    virtual bool open(yarp::os::Searchable &options);
};
 
 
class iCubLegCartesianSolver : public CartesianSolver
{
protected:
    virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
 
public:
    iCubLegCartesianSolver(const std::string &_slvName="legCartSolver") : CartesianSolver(_slvName) { }
};
 
}
 
}
 
#endif