iCub-main
ServerCartesianController.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 __SERVERCARTESIANCONTROLLER_H__
20 #define __SERVERCARTESIANCONTROLLER_H__
21 
22 #include <mutex>
23 #include <condition_variable>
24 #include <string>
25 #include <vector>
26 #include <set>
27 #include <deque>
28 #include <map>
29 
30 #include <yarp/os/all.h>
31 #include <yarp/dev/all.h>
32 #include <yarp/sig/all.h>
33 
34 #include <iCub/ctrl/pids.h>
35 #include <iCub/iKin/iKinHlp.h>
36 #include <iCub/iKin/iKinFwd.h>
37 #include <iCub/iKin/iKinInv.h>
38 
39 #include "SmithPredictor.h"
40 
41 
43 
44 
46 {
47  std::string key;
49 
50  yarp::sig::Vector minAbsVels;
52 };
53 
54 
55 class CartesianCtrlRpcProcessor : public yarp::os::PortReader
56 {
57 protected:
59  bool read(yarp::os::ConnectionReader &connection);
60 
61 public:
63 };
64 
65 
66 class CartesianCtrlCommandPort : public yarp::os::BufferedPort<yarp::os::Bottle>
67 {
68 protected:
70  void onRead(yarp::os::Bottle &command);
71 
72 public:
74 };
75 
76 
78 {
79 protected:
81  yarp::sig::Matrix R;
82 
83 public:
84  TaskRefVelTargetGenerator(const double Ts, const yarp::sig::Vector &x0);
85  void reset(const yarp::sig::Vector &x0);
86  yarp::sig::Vector integrate(const yarp::sig::Vector &vel);
88 };
89 
104 class ServerCartesianController : public yarp::dev::DeviceDriver,
105  public yarp::dev::IMultipleWrapper,
106  public yarp::dev::ICartesianControl,
107  public yarp::os::PeriodicThread,
109 {
110 protected:
111  yarp::dev::PolyDriverList drivers;
112 
113  bool attached;
114  bool connected;
115  bool closed;
129 
130  std::string ctrlName;
131  std::string slvName;
132  std::string kinPart;
133  std::string kinType;
134  int numDrv;
135 
140 
141  yarp::os::Property plantModelProperties;
143 
144  std::deque<DriverDescriptor> lDsc;
145  std::deque<yarp::dev::IControlMode*> lMod;
146  std::deque<yarp::dev::IEncoders*> lEnc;
147  std::deque<yarp::dev::IEncodersTimed*> lEnt;
148  std::deque<yarp::dev::IPidControl*> lPid;
149  std::deque<yarp::dev::IControlLimits*> lLim;
150  std::deque<yarp::dev::IVelocityControl*> lVel;
151  std::deque<yarp::dev::IPositionDirect*> lPos;
152  std::deque<yarp::dev::IPositionControl*> lStp;
153  std::deque<int> lJnt;
154  std::deque<int*> lRmp;
155 
156  unsigned int connectCnt;
157  unsigned int ctrlPose;
159  double targetTol;
160  double trajTime;
161  double pathPerc;
164 
165  double txToken;
166  double rxToken;
171 
172  std::mutex mtx;
173  std::mutex mtx_syncEvent;
174  std::condition_variable cv_syncEvent;
175  yarp::os::Stamp txInfo;
176  yarp::os::Stamp poseInfo;
177  yarp::os::Stamp eventInfo;
178  yarp::os::Stamp debugInfo;
179 
180  yarp::sig::Vector xdes;
181  yarp::sig::Vector qdes;
182  yarp::sig::Vector xdot_set;
183  yarp::sig::Vector velCmd;
184  yarp::sig::Vector fb;
185  yarp::sig::Vector q0;
186 
187  yarp::os::BufferedPort<yarp::os::Bottle> portSlvIn;
188  yarp::os::BufferedPort<yarp::os::Bottle> portSlvOut;
189  yarp::os::RpcClient portSlvRpc;
190 
191  yarp::os::BufferedPort<yarp::sig::Vector> portState;
192  yarp::os::BufferedPort<yarp::os::Bottle> portEvent;
193  yarp::os::BufferedPort<yarp::os::Bottle> portDebugInfo;
194  yarp::os::RpcServer portRpc;
195 
198 
199  struct Context
200  {
201  yarp::sig::Vector dof;
202  yarp::sig::Vector restPos;
203  yarp::sig::Vector restWeights;
204  yarp::sig::Vector tip_x;
205  yarp::sig::Vector tip_o;
206  yarp::sig::Matrix limits;
207  double trajTime;
208  double tol;
209  bool mode;
211  double straightness;
212  std::string posePriority;
213  yarp::os::Value task_2;
214  yarp::os::Bottle solverConvergence;
215  };
216 
218  std::map<int,Context> contextMap;
219  std::map<std::string,yarp::dev::CartesianEvent*> eventsMap;
220 
221  std::multiset<double> motionOngoingEvents;
222  std::multiset<double> motionOngoingEventsCurrent;
223 
224  yarp::os::Bottle sendCtrlCmdMultipleJointsPosition();
225  yarp::os::Bottle sendCtrlCmdMultipleJointsVelocity();
226  yarp::os::Bottle sendCtrlCmdSingleJointPosition();
227  yarp::os::Bottle sendCtrlCmdSingleJointVelocity();
228  yarp::os::Bottle (ServerCartesianController::*sendCtrlCmd)();
229 
230  void init();
231  void openPorts();
232  void closePorts();
233  bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply);
234  bool alignJointsBounds();
235  double getFeedback(yarp::sig::Vector &_fb);
236  void createController();
237  bool getNewTarget();
238  bool areJointsHealthyAndSet(std::vector<int> &jointsToSet);
239  void setJointsCtrlMode(const std::vector<int> &jointsToSet);
240  void stopLimb(const bool execStopPosition=true);
241  bool goTo(unsigned int _ctrlPose, const yarp::sig::Vector &xd, const double t, const bool latchToken=false);
242  bool deleteContexts(yarp::os::Bottle *contextIdList);
243  void notifyEvent(const std::string &event, const double checkPoint=-1.0);
246  bool registerMotionOngoingEvent(const double checkPoint);
247  bool unregisterMotionOngoingEvent(const double checkPoint);
248  yarp::os::Bottle listMotionOngoingEvents();
249 
250  bool threadInit();
251  void afterStart(bool s);
252  void run();
253  void threadRelease();
254 
257 
258  bool stopControlHelper();
259  bool setTrackingModeHelper(const bool f);
260  bool setTrajTimeHelper(const double t);
261  bool setInTargetTolHelper(const double tol);
262  bool isInTargetHelper();
263 
264  bool getTask2ndOptions(yarp::os::Value &v);
265  bool setTask2ndOptions(const yarp::os::Value &v);
266  bool getSolverConvergenceOptions(yarp::os::Bottle &options);
267  bool setSolverConvergenceOptions(const yarp::os::Bottle &options);
268 
269 public:
271  ServerCartesianController(yarp::os::Searchable &config);
272 
273  bool open(yarp::os::Searchable &config);
274  bool close();
275 
276  bool pingSolver();
277  bool connectToSolver();
278 
279  bool attachAll(const yarp::dev::PolyDriverList &p);
280  bool detachAll();
281 
282  bool setTrackingMode(const bool f);
283  bool getTrackingMode(bool *f);
284  bool setReferenceMode(const bool f);
285  bool getReferenceMode(bool *f);
286  bool setPosePriority(const std::string &p);
287  bool getPosePriority(std::string &p);
288  bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
289  bool getPose(const int axis, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL);
290  bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0);
291  bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0);
292  bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0);
293  bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0);
294  bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
295  bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat,
296  yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
297  bool askForPose(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, const yarp::sig::Vector &od,
298  yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
299  bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat,
300  yarp::sig::Vector &qdhat);
301  bool askForPosition(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat,
302  yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat);
303  bool getDOF(yarp::sig::Vector &curDof);
304  bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof);
305  bool getRestPos(yarp::sig::Vector &curRestPos);
306  bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos);
307  bool getRestWeights(yarp::sig::Vector &curRestWeights);
308  bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights);
309  bool getLimits(const int axis, double *min, double *max);
310  bool setLimits(const int axis, const double min, const double max);
311  bool getTrajTime(double *t);
312  bool setTrajTime(const double t);
313  bool getInTargetTol(double *tol);
314  bool setInTargetTol(const double tol);
315  bool getJointsVelocities(yarp::sig::Vector &qdot);
316  bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot);
317  bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot);
318  bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o);
319  bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o);
320  bool removeTipFrame();
321  bool checkMotionDone(bool *f);
322  bool waitMotionDone(const double period=0.1, const double timeout=0.0);
323  bool stopControl();
324  bool storeContext(int *id);
325  bool restoreContext(const int id);
326  bool deleteContext(const int id);
327  bool getInfo(yarp::os::Bottle &info);
328  bool registerEvent(yarp::dev::CartesianEvent &event);
329  bool unregisterEvent(yarp::dev::CartesianEvent &event);
330  bool tweakSet(const yarp::os::Bottle &options);
331  bool tweakGet(yarp::os::Bottle &options);
332 
333  virtual ~ServerCartesianController();
334 };
335 
336 
337 #endif
338 
339 
ServerCartesianController * server
void onRead(yarp::os::Bottle &command)
CartesianCtrlCommandPort(ServerCartesianController *server)
ServerCartesianController * server
CartesianCtrlRpcProcessor(ServerCartesianController *server)
bool read(yarp::os::ConnectionReader &connection)
servercartesiancontroller : implements the server part of the Cartesian Interface.
bool tweakSet(const yarp::os::Bottle &options)
yarp::os::BufferedPort< yarp::os::Bottle > portEvent
iCub::iKin::MultiRefMinJerkCtrl * ctrl
bool deleteContexts(yarp::os::Bottle *contextIdList)
std::deque< yarp::dev::IVelocityControl * > lVel
yarp::os::Bottle sendCtrlCmdSingleJointPosition()
ServerCartesianController(yarp::os::Searchable &config)
yarp::os::BufferedPort< yarp::sig::Vector > portState
void notifyEvent(const std::string &event, const double checkPoint=-1.0)
std::deque< yarp::dev::IPositionControl * > lStp
yarp::os::Bottle sendCtrlCmdMultipleJointsVelocity()
bool askForPosition(const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
std::map< int, Context > contextMap
bool getDOF(yarp::sig::Vector &curDof)
yarp::os::Bottle sendCtrlCmdMultipleJointsPosition()
bool getPose(const int axis, yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
std::deque< DriverDescriptor > lDsc
bool getRestWeights(yarp::sig::Vector &curRestWeights)
yarp::os::BufferedPort< yarp::os::Bottle > portDebugInfo
CartesianCtrlRpcProcessor * rpcProcessor
bool getTipFrame(yarp::sig::Vector &x, yarp::sig::Vector &o)
bool goToPosition(const yarp::sig::Vector &xd, const double t=0.0)
bool attachAll(const yarp::dev::PolyDriverList &p)
std::deque< yarp::dev::IEncodersTimed * > lEnt
iCub::iKin::iKinChain * chainState
bool setLimits(const int axis, const double min, const double max)
double getFeedback(yarp::sig::Vector &_fb)
bool open(yarp::os::Searchable &config)
yarp::os::Bottle sendCtrlCmdSingleJointVelocity()
bool askForPose(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
bool setRestWeights(const yarp::sig::Vector &newRestWeights, yarp::sig::Vector &curRestWeights)
yarp::os::Bottle(ServerCartesianController::* sendCtrlCmd)()
std::multiset< double > motionOngoingEventsCurrent
bool attachTipFrame(const yarp::sig::Vector &x, const yarp::sig::Vector &o)
yarp::os::BufferedPort< yarp::os::Bottle > portSlvOut
bool getTask2ndOptions(yarp::os::Value &v)
std::deque< yarp::dev::IControlMode * > lMod
bool registerEvent(yarp::dev::CartesianEvent &event)
bool getLimits(const int axis, double *min, double *max)
bool unregisterMotionOngoingEvent(const double checkPoint)
std::deque< yarp::dev::IControlLimits * > lLim
void setJointsCtrlMode(const std::vector< int > &jointsToSet)
bool tweakGet(yarp::os::Bottle &options)
bool waitMotionDone(const double period=0.1, const double timeout=0.0)
bool setTask2ndOptions(const yarp::os::Value &v)
bool setInTargetTolHelper(const double tol)
bool respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
bool getSolverConvergenceOptions(yarp::os::Bottle &options)
bool setPosePriority(const std::string &p)
bool askForPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
bool getTaskVelocities(yarp::sig::Vector &xdot, yarp::sig::Vector &odot)
bool goToPoseSync(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
bool askForPosition(const yarp::sig::Vector &q0, const yarp::sig::Vector &xd, yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
bool getInfo(yarp::os::Bottle &info)
bool setSolverConvergenceOptions(const yarp::os::Bottle &options)
TaskRefVelTargetGenerator * taskRefVelTargetGen
bool goToPose(const yarp::sig::Vector &xd, const yarp::sig::Vector &od, const double t=0.0)
std::map< std::string, yarp::dev::CartesianEvent * > eventsMap
bool unregisterEvent(yarp::dev::CartesianEvent &event)
bool goTo(unsigned int _ctrlPose, const yarp::sig::Vector &xd, const double t, const bool latchToken=false)
bool areJointsHealthyAndSet(std::vector< int > &jointsToSet)
yarp::os::BufferedPort< yarp::os::Bottle > portSlvIn
bool setTaskVelocities(const yarp::sig::Vector &xdot, const yarp::sig::Vector &odot)
std::multiset< double > motionOngoingEvents
bool goToPositionSync(const yarp::sig::Vector &xd, const double t=0.0)
CartesianCtrlCommandPort * portCmd
bool setRestPos(const yarp::sig::Vector &newRestPos, yarp::sig::Vector &curRestPos)
iCub::iKin::iKinChain * chainPlan
std::deque< yarp::dev::IPositionDirect * > lPos
std::deque< yarp::dev::IEncoders * > lEnc
void stopLimb(const bool execStopPosition=true)
yarp::dev::PolyDriverList drivers
std::condition_variable cv_syncEvent
bool setDOF(const yarp::sig::Vector &newDof, yarp::sig::Vector &curDof)
bool getRestPos(yarp::sig::Vector &curRestPos)
bool getPose(yarp::sig::Vector &x, yarp::sig::Vector &o, yarp::os::Stamp *stamp=NULL)
bool getDesired(yarp::sig::Vector &xdhat, yarp::sig::Vector &odhat, yarp::sig::Vector &qdhat)
bool getJointsVelocities(yarp::sig::Vector &qdot)
bool registerMotionOngoingEvent(const double checkPoint)
std::deque< yarp::dev::IPidControl * > lPid
void reset(const yarp::sig::Vector &x0)
yarp::sig::Vector integrate(const yarp::sig::Vector &vel)
TaskRefVelTargetGenerator(const double Ts, const yarp::sig::Vector &x0)
A class for defining a saturated integrator based on Tustin formula: .
Definition: pids.h:48
Helper class providing useful methods to deal with Cartesian Solver options.
Definition: iKinHlp.h:48
A class derived from iKinCtrl implementing the multi-referential approach (pdf).
Definition: iKinInv.h:760
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:355
A class for defining generic Limb.
Definition: iKinFwd.h:874
static int v
Definition: iCub_Sim.cpp:42
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
yarp::sig::Vector minAbsVels