iCub-main
ClientCartesianController.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010 RobotCub Consortium, European Commission FP6 Project IST-004370
3  * Author: Ugo Pattacini
4  * email: ugo.pattacini@iit.it
5  * website: www.robotcub.org
6  * Permission is granted to copy, distribute, and/or modify this program
7  * under the terms of the GNU General Public License, version 2 or any
8  * later version published by the Free Software Foundation.
9  *
10  * A copy of the license can be found at
11  * http://www.robotcub.org/icub/license/gpl.txt
12  *
13  * This program is distributed in the hope that it will be useful, but
14  * WITHOUT ANY WARRANTY; without even the implied warranty of
15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
16  * Public License for more details
17 */
18 
19 #ifndef __CLIENTCARTESIANCONTROLLER_H__
20 #define __CLIENTCARTESIANCONTROLLER_H__
21 
22 #include <string>
23 #include <set>
24 #include <map>
25 
26 #include <yarp/os/all.h>
27 #include <yarp/dev/all.h>
28 #include <yarp/sig/all.h>
29 
30 #include <iCub/iKin/iKinHlp.h>
31 
32 
33 // forward declaration
35 
36 
37 /************************************************************************/
38 class CartesianEventHandler : public yarp::os::BufferedPort<yarp::os::Bottle>
39 {
40 protected:
42  void onRead(yarp::os::Bottle &event);
43 
44 public:
47 };
48 
49 
63 class ClientCartesianController : public yarp::dev::DeviceDriver,
64  public yarp::dev::ICartesianControl,
66 {
67 protected:
68  bool connected;
69  bool closed;
70 
71  double timeout;
73 
74  yarp::sig::Vector pose;
75  yarp::os::Stamp poseStamp;
76 
77  yarp::os::BufferedPort<yarp::sig::Vector> portState;
78  yarp::os::BufferedPort<yarp::os::Bottle> portCmd;
79  yarp::os::RpcClient portRpc;
80 
81  std::set<int> contextIdList;
82  std::map<std::string,yarp::dev::CartesianEvent*> eventsMap;
84  friend class CartesianEventHandler;
85 
86  void init();
87  bool deleteContexts();
88  void eventHandling(yarp::os::Bottle &event);
89  bool getInfoHelper(yarp::os::Bottle &info);
90 
91 public:
93  ClientCartesianController(yarp::os::Searchable &config);
94 
95  bool open(yarp::os::Searchable &config);
96  bool close();
97 
98  bool setTrackingMode(const bool f);
99  bool getTrackingMode(bool *f);
100  bool setReferenceMode(const bool f);
101  bool getReferenceMode(bool *f);
102  bool setPosePriority(const std::string &p);
104  bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
105  bool getPose(const int axis, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
106  bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0);
107  bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0);
108  bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0);
109  bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0);
110  bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat,yarp::sig::Vector &qdhat);
111  bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat,
112  yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
113  bool askForPose(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, const yarp::sig::Vector &od,
114  yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
115  bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat,
116  yarp::sig::Vector &qdhat);
117  bool askForPosition(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat,
118  yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
119  bool getDOF(yarp::sig::Vector &curDof);
120  bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof);
121  bool getRestPos(yarp::sig::Vector &curRestPos);
122  bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos);
123  bool getRestWeights(yarp::sig::Vector &curRestWeights);
124  bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights);
125  bool getLimits(const int axis, double *min, double *max);
126  bool setLimits(const int axis, const double min, const double max);
127  bool getTrajTime(double *t);
128  bool setTrajTime(const double t);
129  bool getInTargetTol(double *tol);
130  bool setInTargetTol(const double tol);
131  bool getJointsVelocities(yarp::sig::Vector &qdot);
132  bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot);
133  bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot);
134  bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o);
135  bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o);
136  bool removeTipFrame();
137  bool checkMotionDone(bool *f);
138  bool waitMotionDone(const double period=0.1, const double timeout=0.0);
139  bool stopControl();
140  bool storeContext(int *id);
141  bool restoreContext(const int id);
142  bool deleteContext(const int id);
143  bool getInfo(yarp::os::Bottle &info);
144  bool registerEvent(yarp::dev::CartesianEvent &event);
145  bool unregisterEvent(yarp::dev::CartesianEvent &event);
146  bool tweakSet(const yarp::os::Bottle &options);
147  bool tweakGet(yarp::os::Bottle &options);
148 
149  virtual ~ClientCartesianController();
150 };
151 
152 
153 #endif
154 
155 
ClientCartesianController::getTaskVelocities
bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
Definition: ClientCartesianController.cpp:1070
ClientCartesianController::getDOF
bool getDOF(yarp::sig::Vector &curDof)
Definition: ClientCartesianController.cpp:679
ClientCartesianController::ClientCartesianController
ClientCartesianController()
Definition: ClientCartesianController.cpp:60
ClientCartesianController::eventsMap
std::map< std::string, yarp::dev::CartesianEvent * > eventsMap
Definition: ClientCartesianController.h:82
ClientCartesianController::lastPoseMsgArrivalTime
double lastPoseMsgArrivalTime
Definition: ClientCartesianController.h:72
ClientCartesianController::getTrajTime
bool getTrajTime(double *t)
Definition: ClientCartesianController.cpp:937
ClientCartesianController::checkMotionDone
bool checkMotionDone(bool *f)
Definition: ClientCartesianController.cpp:1216
iCub::action::log::info
@ info
Definition: actionPrimitives.cpp:64
ClientCartesianController::getPose
bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
ClientCartesianController::setTaskVelocities
bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
Definition: ClientCartesianController.cpp:1107
ClientCartesianController::getRestWeights
bool getRestWeights(yarp::sig::Vector &curRestWeights)
Definition: ClientCartesianController.cpp:815
iCub::iKin::CartesianHelper
Definition: iKinHlp.h:47
ClientCartesianController::closed
bool closed
Definition: ClientCartesianController.h:69
ClientCartesianController::setTrajTime
bool setTrajTime(const double t)
Definition: ClientCartesianController.cpp:966
ClientCartesianController::getJointsVelocities
bool getJointsVelocities(yarp::sig::Vector &qdot)
Definition: ClientCartesianController.cpp:1037
strain::dsp::fsc::min
const FSC min
Definition: strain.h:49
ClientCartesianController::setRestWeights
bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
Definition: ClientCartesianController.cpp:848
ClientCartesianController::portRpc
yarp::os::RpcClient portRpc
Definition: ClientCartesianController.h:79
ClientCartesianController::removeTipFrame
bool removeTipFrame()
Definition: ClientCartesianController.cpp:1209
CartesianEventHandler::CartesianEventHandler
CartesianEventHandler()
Definition: ClientCartesianController.h:45
ClientCartesianController::close
bool close()
Definition: ClientCartesianController.cpp:180
ClientCartesianController::askForPosition
bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
ClientCartesianController::deleteContexts
bool deleteContexts()
Definition: ClientCartesianController.cpp:1363
ClientCartesianController::unregisterEvent
bool unregisterEvent(yarp::dev::CartesianEvent &event)
Definition: ClientCartesianController.cpp:1513
ClientCartesianController::getInTargetTol
bool getInTargetTol(double *tol)
Definition: ClientCartesianController.cpp:987
ClientCartesianController::goToPoseSync
bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ClientCartesianController.cpp:497
p
p
Definition: show_eyes_axes.m:23
ClientCartesianController::contextIdList
std::set< int > contextIdList
Definition: ClientCartesianController.h:81
ClientCartesianController::storeContext
bool storeContext(int *id)
Definition: ClientCartesianController.cpp:1289
ClientCartesianController
clientcartesiancontroller : implements the client part of the Cartesian Interface.
Definition: ClientCartesianController.h:63
strain::dsp::fsc::max
const FSC max
Definition: strain.h:48
ClientCartesianController::getLimits
bool getLimits(const int axis, double *min, double *max)
Definition: ClientCartesianController.cpp:881
ClientCartesianController::getDesired
bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
Definition: ClientCartesianController.cpp:551
CartesianEventHandler::setInterface
void setInterface(ClientCartesianController *interface)
Definition: ClientCartesianController.cpp:51
ClientCartesianController::pose
yarp::sig::Vector pose
Definition: ClientCartesianController.h:74
ClientCartesianController::goToPositionSync
bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ClientCartesianController.cpp:526
ClientCartesianController::portState
yarp::os::BufferedPort< yarp::sig::Vector > portState
Definition: ClientCartesianController.h:77
CartesianEventHandler::interface
ClientCartesianController * interface
Definition: ClientCartesianController.h:41
ClientCartesianController::tweakSet
bool tweakSet(const yarp::os::Bottle &options)
Definition: ClientCartesianController.cpp:1549
ClientCartesianController::attachTipFrame
bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
Definition: ClientCartesianController.cpp:1132
iKinHlp.h
ClientCartesianController::init
void init()
Definition: ClientCartesianController.cpp:75
ClientCartesianController::setRestPos
bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
Definition: ClientCartesianController.cpp:782
ClientCartesianController::~ClientCartesianController
virtual ~ClientCartesianController()
Definition: ClientCartesianController.cpp:1601
CartesianEventHandler::onRead
void onRead(yarp::os::Bottle &event)
Definition: ClientCartesianController.cpp:43
ClientCartesianController::setDOF
bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
Definition: ClientCartesianController.cpp:712
ClientCartesianController::stopControl
bool stopControl()
Definition: ClientCartesianController.cpp:1270
x
x
Definition: compute_ekf_sym.m:21
ClientCartesianController::timeout
double timeout
Definition: ClientCartesianController.h:71
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
ClientCartesianController::portCmd
yarp::os::BufferedPort< yarp::os::Bottle > portCmd
Definition: ClientCartesianController.h:78
ClientCartesianController::setInTargetTol
bool setInTargetTol(const double tol)
Definition: ClientCartesianController.cpp:1016
ClientCartesianController::eventHandling
void eventHandling(yarp::os::Bottle &event)
Definition: ClientCartesianController.cpp:1425
ClientCartesianController::setTrackingMode
bool setTrackingMode(const bool f)
Definition: ClientCartesianController.cpp:206
ClientCartesianController::getTrackingMode
bool getTrackingMode(bool *f)
Definition: ClientCartesianController.cpp:228
f
f
Definition: compute_ekf_sym.m:22
ClientCartesianController::getInfoHelper
bool getInfoHelper(yarp::os::Bottle &info)
Definition: ClientCartesianController.cpp:1387
CartesianEventHandler
Definition: ClientCartesianController.h:38
ClientCartesianController::getPosePriority
bool getPosePriority(std::string &p)
Definition: ClientCartesianController.cpp:339
ClientCartesianController::setPosePriority
bool setPosePriority(const std::string &p)
Definition: ClientCartesianController.cpp:318
ClientCartesianController::getTipFrame
bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
Definition: ClientCartesianController.cpp:1172
ClientCartesianController::tweakGet
bool tweakGet(yarp::os::Bottle &options)
Definition: ClientCartesianController.cpp:1570
ClientCartesianController::setLimits
bool setLimits(const int axis, const double min, const double max)
Definition: ClientCartesianController.cpp:913
ClientCartesianController::registerEvent
bool registerEvent(yarp::dev::CartesianEvent &event)
Definition: ClientCartesianController.cpp:1477
ClientCartesianController::getRestPos
bool getRestPos(yarp::sig::Vector &curRestPos)
Definition: ClientCartesianController.cpp:749
ClientCartesianController::connected
bool connected
Definition: ClientCartesianController.h:68
ClientCartesianController::portEvents
CartesianEventHandler portEvents
Definition: ClientCartesianController.h:83
ClientCartesianController::waitMotionDone
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
Definition: ClientCartesianController.cpp:1251
ClientCartesianController::goToPose
bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
Definition: ClientCartesianController.cpp:447
ClientCartesianController::getReferenceMode
bool getReferenceMode(bool *f)
Definition: ClientCartesianController.cpp:284
ClientCartesianController::goToPosition
bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0)
Definition: ClientCartesianController.cpp:474
ClientCartesianController::deleteContext
bool deleteContext(const int id)
Definition: ClientCartesianController.cpp:1337
ClientCartesianController::open
bool open(yarp::os::Searchable &config)
Definition: ClientCartesianController.cpp:90
ClientCartesianController::restoreContext
bool restoreContext(const int id)
Definition: ClientCartesianController.cpp:1317
ClientCartesianController::poseStamp
yarp::os::Stamp poseStamp
Definition: ClientCartesianController.h:75
ClientCartesianController::setReferenceMode
bool setReferenceMode(const bool f)
Definition: ClientCartesianController.cpp:262
ClientCartesianController::getInfo
bool getInfo(yarp::os::Bottle &info)
Definition: ClientCartesianController.cpp:1415
ClientCartesianController::askForPose
bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)