d4c
d4c_server.h
1 /*
2  * Copyright (C) 2013 iCub Facility - Istituto Italiano di Tecnologia
3  * Authors: Ilaria Gori, Ugo Pattacini
4  * email: ilaria.gori@iit.it, ugo.pattacini@iit.it
5  * Permission is granted to copy, distribute, and/or modify this program
6  * under the terms of the GNU General Public License, version 2 or any
7  * later version published by the Free Software Foundation.
8  *
9  * A copy of the license can be found at
10  * http://www.robotcub.org/icub/license/gpl.txt
11  *
12  * This program is distributed in the hope that it will be useful, but
13  * WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
15  * Public License for more details
16 */
17 
18 #ifndef __D4C_SERVER_H__
19 #define __D4C_SERVER_H__
20 
21 #include <sstream>
22 #include <string>
23 #include <map>
24 #include <deque>
25 
26 #include <yarp/os/all.h>
27 #include <yarp/dev/all.h>
28 #include <yarp/sig/all.h>
29 
30 #include <iCub/ctrl/pids.h>
31 #include <iCub/d4c/d4c.h>
32 
33 namespace iCub
34 {
35 
36 namespace d4c
37 {
38 
39 // ancestral class
40 class Item
41 {
42 public:
43  std::string name;
44  std::string type;
45  bool active;
46 
47  yarp::sig::Vector center;
48  yarp::sig::Vector orientation;
49  yarp::sig::Vector radius;
50  yarp::sig::Vector color;
51 
52  Item();
53  virtual bool fromProperty(const yarp::os::Property &options);
54  virtual yarp::os::Property toProperty() const;
55  virtual yarp::sig::Vector getField(const yarp::sig::Vector &x,
56  const yarp::sig::Vector &xdot)=0;
57  virtual ~Item() { }
58 };
59 
60 
61 // target with mass-spring-damper force field
62 class Target_MSD : public Item
63 {
64 public:
65  double K;
66  double D;
67 
68  Target_MSD();
69  bool fromProperty(const yarp::os::Property &options);
70  yarp::os::Property toProperty() const;
71  yarp::sig::Vector getField(const yarp::sig::Vector &x,
72  const yarp::sig::Vector &xdot);
73 };
74 
75 
76 // obstacle with gaussian force field
77 class Obstacle_Gaussian : public Item
78 {
79 public:
80  double G;
81  bool cut_tails;
82 
83  Obstacle_Gaussian();
84  bool fromProperty(const yarp::os::Property &options);
85  yarp::os::Property toProperty() const;
86  yarp::sig::Vector getField(const yarp::sig::Vector &x,
87  const yarp::sig::Vector &xdot);
88 };
89 
90 
91 class D4CServer; // forward declaration
92 class GuiReporter : public yarp::os::PortReport
93 {
94 private:
95  D4CServer *server;
96 
97 public:
98  GuiReporter();
99  void setServer(D4CServer *server);
100  void report(const yarp::os::PortInfo &info);
101 };
102 
103 
104 class D4CServer : public D4C,
105  public yarp::os::RateThread,
106  public yarp::os::PortReader
107 {
108 protected:
109  bool isOpen;
110  bool fieldEnabled;
111  bool controlEnabled;
112  bool simulationEnabled;
113  bool simulationFirstStep;
114  bool offlineMode;
115  bool initIntegration;
116  bool doInitGuiTrajectory;
117  int verbosity;
118  int period;
119 
120  std::string device;
121  std::string name;
122  std::string robot;
123  std::string part;
124  std::string activeIF;
125 
126  std::map<int,Item*> table;
127  int itCnt;
128  int doTrajectoryCnt;
129  double t0;
130 
131  yarp::sig::Vector xdot;
132  yarp::sig::Vector x;
133  yarp::sig::Vector xhat;
134  yarp::sig::Vector qhat;
135 
136  yarp::sig::Matrix toolFrame;
137  yarp::sig::Matrix invToolFrame;
138 
139  iCub::ctrl::Integrator If;
140  iCub::ctrl::Integrator Iv;
141 
142  yarp::os::Mutex mutex;
143  yarp::dev::PolyDriver dCtrlLeft;
144  yarp::dev::ICartesianControl *iCtrlLeft;
145  yarp::dev::PolyDriver dCtrlRight;
146  yarp::dev::ICartesianControl *iCtrlRight;
147  yarp::dev::ICartesianControl *iCtrlActive;
148 
149  yarp::os::BufferedPort<yarp::os::Property> data;
150  yarp::os::BufferedPort<yarp::os::Bottle> gui;
151  yarp::os::RpcServer rpc;
152 
153  friend class GuiReporter;
154  GuiReporter reporter;
155 
156  class GuiRequest
157  {
158  std::string type;
159  std::string name;
160  std::map<int,Item*>::iterator iter;
161 
162  GuiRequest(); // not accessible
163 
164  public:
165 
166  GuiRequest(const std::string &type, std::map<int,Item*>::iterator &iter)
167  {
168  std::ostringstream name;
169  name<<iter->second->name<<":"<<std::hex<<iter->first;
170 
171  this->type=type;
172  this->iter=iter;
173  this->name=name.str();
174  }
175 
176  std::string getType() const { return type; }
177  std::string getName() const { return name; }
178  std::map<int,Item*>::iterator getIter() const { return iter; }
179 
180  bool operator==(const GuiRequest &req)
181  {
182  return ((type==req.type)&&(iter==req.iter));
183  }
184  };
185 
186  std::deque<GuiRequest> guiQueue;
187 
188  virtual void printMessage(const int logtype, const int level,
189  const char *format, ...) const;
190 
191  Item* itemFactory(const yarp::os::Property &options);
192  bool read(yarp::os::ConnectionReader &connection);
193  void scheduleInitGuiTrajectory();
194  void initGuiTrajectory();
195  void updateGuiTrajectory();
196  void eraseGuiTrajectory();
197  void updateGuiItem(const GuiRequest &req);
198  void eraseGuiItem(const GuiRequest &req);
199  void pushUpdateGuiItem(std::map<int,Item*>::iterator &it);
200  void pushEraseGuiItem(std::map<int,Item*>::iterator &it);
201  void handleGuiQueue();
202  void run();
203 
204  yarp::os::Property prepareData();
205  void getTargetForCartesianIF(yarp::sig::Vector &pos, yarp::sig::Vector &orien);
206 
207 public:
208  D4CServer();
209  bool open(const yarp::os::Property &options);
210  void close();
211  bool addItem(const yarp::os::Property &options, int &item);
212  bool eraseItem(const int item);
213  bool clearItems();
214  bool getItems(yarp::os::Bottle &items);
215  bool setProperty(const int item, const yarp::os::Property &options);
216  bool getProperty(const int item, yarp::os::Property &options);
217  bool enableField();
218  bool disableField();
219  bool getFieldStatus(bool &status);
220  bool enableControl();
221  bool disableControl();
222  bool getControlStatus(bool &status);
223  bool enableSimulation();
224  bool disableSimulation();
225  bool getSimulationStatus(bool &status);
226  bool setPeriod(const int period);
227  bool getPeriod(int &period);
228  bool setPointStateToTool();
229  bool attachToolFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o);
230  bool getToolFrame(yarp::sig::Vector &x, yarp::sig::Vector &o);
231  bool removeToolFrame();
232  bool getTool(yarp::sig::Vector &x, yarp::sig::Vector &o);
233  bool setPointState(const yarp::sig::Vector &x, const yarp::sig::Vector &o,
234  const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot);
235  bool setPointOrientation(const yarp::sig::Vector &o, const yarp::sig::Vector &odot);
236  bool getPointState(yarp::sig::Vector &x, yarp::sig::Vector &o,
237  yarp::sig::Vector &xdot, yarp::sig::Vector &odot);
238  bool getField(yarp::sig::Vector &field);
239  bool getSimulation(yarp::sig::Vector &xhat, yarp::sig::Vector &ohat,
240  yarp::sig::Vector &qhat);
241  bool getActiveIF(std::string &activeIF);
242  bool setActiveIF(const std::string &activeIF);
243  bool getTrajectory(std::deque<yarp::sig::Vector> &trajPos,
244  std::deque<yarp::sig::Vector> &trajOrien,
245  const unsigned int maxIterations=D4C_DEFAULT_MAXITERATIONS,
246  const double Ts=D4C_DEFAULT_TS_DISABLED);
247  bool executeTrajectory(const std::deque<yarp::sig::Vector> &trajPos,
248  const std::deque<yarp::sig::Vector> &trajOrien,
249  const double trajTime);
250  virtual ~D4CServer();
251 };
252 
253 }
254 
255 }
256 
257 #endif
258 
259 
iCub
Definition: d4c.h:43
Generated on Sun May 31 2015 15:17:45 for d4c by   doxygen 1.8.9