iCub-main
iKinSlv.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT)
3  * Copyright (C) 2006-2010 RobotCub Consortium
4  * All rights reserved.
5  *
6  * This software may be modified and distributed under the terms
7  * of the BSD-3-Clause license. See the accompanying LICENSE file for
8  * details.
9 */
10 
237 #ifndef __IKINSLV_H__
238 #define __IKINSLV_H__
239 
240 #include <mutex>
241 #include <condition_variable>
242 #include <string>
243 #include <deque>
244 
245 #include <yarp/os/BufferedPort.h>
246 #include <yarp/os/PeriodicThread.h>
247 #include <yarp/sig/Vector.h>
248 #include <yarp/sig/Matrix.h>
249 
250 #include <yarp/dev/ControlBoardInterfaces.h>
251 #include <yarp/dev/PolyDriver.h>
252 
253 #include <iCub/iKin/iKinHlp.h>
254 #include <iCub/iKin/iKinIpOpt.h>
255 
256 
257 namespace iCub
258 {
259 
260 namespace iKin
261 {
262 
263 class CartesianSolver;
264 
265 class RpcProcessor : public yarp::os::PortReader
266 {
267 protected:
269 
270  virtual bool read(yarp::os::ConnectionReader &connection);
271 
272 public:
273  RpcProcessor(CartesianSolver *_slv) : slv(_slv) { }
274 };
275 
276 
277 class InputPort : public yarp::os::BufferedPort<yarp::os::Bottle>
278 {
279 protected:
281 
282  std::mutex mtx;
283 
284  bool contMode;
285  bool isNew;
286  size_t maxLen;
287  int pose;
288 
289  double token;
290  double *pToken;
291 
292  yarp::sig::Vector dof;
293  yarp::sig::Vector xd;
294 
295  virtual void onRead(yarp::os::Bottle &b);
296 
297 public:
298  InputPort(CartesianSolver *_slv);
299 
300  int &get_pose() { return pose; }
301  bool &get_contMode() { return contMode; }
302  double *get_tokenPtr() { return pToken; }
303  yarp::sig::Vector get_dof();
304  yarp::sig::Vector get_xd();
305 
306  void set_dof(const yarp::sig::Vector &_dof);
307  void reset_xd(const yarp::sig::Vector &_xd);
308  bool isNewDataEvent();
309  bool handleTarget(yarp::os::Bottle *b);
310  bool handleDOF(yarp::os::Bottle *b);
311  bool handlePose(const int newPose);
312  bool handleMode(const int newMode);
313 };
314 
315 
317 {
318 protected:
320 
321 public:
322  SolverCallback(CartesianSolver *_slv) : slv(_slv) { }
323 
324  virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q);
325 };
326 
327 
329 {
333  std::deque<yarp::os::Property> prp;
334  std::deque<bool> rvs;
335  int num;
336 };
337 
338 
344 class CartesianSolver : public yarp::os::PeriodicThread,
345  protected CartesianHelper
346 {
347 protected:
349  std::deque<yarp::dev::PolyDriver*> drv;
350  std::deque<yarp::dev::IControlLimits*> lim;
351  std::deque<yarp::dev::IEncoders*> enc;
352  std::deque<int> jnt;
353  std::deque<int*> rmp;
354 
357 
359  yarp::os::Port *rpcPort;
361  yarp::os::BufferedPort<yarp::os::Bottle> *outPort;
362  std::mutex mtx;
363 
364  std::string slvName;
365  std::string type;
366  unsigned int period;
367  unsigned int ctrlPose;
368  bool fullDOF;
371  bool closing;
372  bool closed;
374  bool verbosity;
379  double token;
380  double *pToken;
381 
382  yarp::sig::Matrix hwLimits;
383  yarp::sig::Matrix swLimits;
384 
385  yarp::sig::Vector unctrlJointsOld;
386  yarp::sig::Vector dof;
387 
388  yarp::sig::Vector restJntPos;
389  yarp::sig::Vector restWeights;
390 
391  yarp::sig::Vector xd_2ndTask;
392  yarp::sig::Vector w_2ndTask;
393 
394  yarp::sig::Vector qd_3rdTask;
395  yarp::sig::Vector w_3rdTask;
396  yarp::sig::Vector idx_3rdTask;
397 
398  std::mutex mtx_dofEvent;
399  std::condition_variable cv_dofEvent;
400 
401  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options)=0;
402  virtual yarp::sig::Vector solve(yarp::sig::Vector &xd);
403 
404  virtual yarp::sig::Vector &encodeDOF();
405  virtual bool decodeDOF(const yarp::sig::Vector &_dof);
406 
407  virtual bool handleJointsRestPosition(const yarp::os::Bottle *options,
408  yarp::os::Bottle *reply=NULL);
409  virtual bool handleJointsRestWeights(const yarp::os::Bottle *options,
410  yarp::os::Bottle *reply=NULL);
411 
412  yarp::dev::PolyDriver *waitPart(const yarp::os::Property &partOpt);
413 
414  bool isNewDOF(const yarp::sig::Vector &_dof);
415  bool changeDOF(const yarp::sig::Vector &_dof);
416 
417  bool alignJointsBounds();
418  bool setLimits(int axis, double min, double max);
420  void latchUncontrolledJoints(yarp::sig::Vector &joints);
421  void getFeedback(const bool wait=false);
422  void initPos();
423  void lock();
424  void unlock();
425 
426  void waitDOFHandling();
427  void postDOFHandling();
428  void fillDOFInfo(yarp::os::Bottle &reply);
429  void send(const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok);
430  void printInfo(const std::string &typ, const yarp::sig::Vector &xd, const yarp::sig::Vector &x,
431  const yarp::sig::Vector &q, const double t);
432 
433  virtual void prepareJointsRestTask();
434  virtual void respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply);
435  virtual bool threadInit();
436  virtual void afterStart(bool);
437  virtual void run();
438  virtual void threadRelease();
439 
440  friend class RpcProcessor;
441  friend class InputPort;
442  friend class SolverCallback;
443 
444 public:
459  CartesianSolver(const std::string &_slvName);
460 
531  virtual bool open(yarp::os::Searchable &options);
532 
537  virtual void interrupt();
538 
542  virtual void close();
543 
549  virtual bool isClosed() const { return closed; }
550 
559  virtual bool &getTimeoutFlag() { return timeout_detected; }
560 
564  virtual void suspend();
565 
569  virtual void resume();
570 
574  virtual ~CartesianSolver();
575 };
576 
577 
585 {
586 protected:
587  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
588  virtual bool decodeDOF(const yarp::sig::Vector &_dof);
589 
590 public:
598  iCubArmCartesianSolver(const std::string &_slvName="armCartSolver") : CartesianSolver(_slvName) { }
599 
600  virtual bool open(yarp::os::Searchable &options);
601 };
602 
603 
611 {
612 protected:
613  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
614 
615 public:
623  iCubLegCartesianSolver(const std::string &_slvName="legCartSolver") : CartesianSolver(_slvName) { }
624 };
625 
626 }
627 
628 }
629 
630 #endif
631 
632 
633 
Helper class providing useful methods to deal with Cartesian Solver options.
Definition: iKinHlp.h:48
Abstract class defining the core of on-line solvers.
Definition: iKinSlv.h:346
yarp::sig::Vector idx_3rdTask
Definition: iKinSlv.h:396
std::deque< yarp::dev::IControlLimits * > lim
Definition: iKinSlv.h:350
yarp::os::BufferedPort< yarp::os::Bottle > * outPort
Definition: iKinSlv.h:361
virtual yarp::sig::Vector & encodeDOF()
Definition: iKinSlv.cpp:1160
virtual bool handleJointsRestWeights(const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
Definition: iKinSlv.cpp:1226
bool setLimits(int axis, double min, double max)
Definition: iKinSlv.cpp:375
std::deque< int > jnt
Definition: iKinSlv.h:352
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition: iKinSlv.cpp:1270
std::condition_variable cv_dofEvent
Definition: iKinSlv.h:399
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)=0
yarp::sig::Vector xd_2ndTask
Definition: iKinSlv.h:391
bool changeDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1458
virtual yarp::sig::Vector solve(yarp::sig::Vector &xd)
Definition: iKinSlv.cpp:1515
virtual void respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply)
Definition: iKinSlv.cpp:515
yarp::sig::Vector w_3rdTask
Definition: iKinSlv.h:395
RpcProcessor * cmdProcessor
Definition: iKinSlv.h:358
virtual void interrupt()
Interrupt the open() method waiting for motor parts to be ready.
Definition: iKinSlv.cpp:1525
yarp::sig::Vector restWeights
Definition: iKinSlv.h:389
virtual void resume()
Resume the solver's main loop.
Definition: iKinSlv.cpp:1630
iKinIpOptMin * slv
Definition: iKinSlv.h:355
CartesianSolver(const std::string &_slvName)
Constructor.
Definition: iKinSlv.cpp:267
std::deque< int * > rmp
Definition: iKinSlv.h:353
yarp::sig::Matrix swLimits
Definition: iKinSlv.h:383
std::deque< yarp::dev::IEncoders * > enc
Definition: iKinSlv.h:351
yarp::sig::Matrix hwLimits
Definition: iKinSlv.h:382
virtual void afterStart(bool)
Definition: iKinSlv.cpp:1607
virtual bool decodeDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1175
yarp::sig::Vector restJntPos
Definition: iKinSlv.h:388
virtual void threadRelease()
Definition: iKinSlv.cpp:1742
yarp::sig::Vector unctrlJointsOld
Definition: iKinSlv.h:385
SolverCallback * clb
Definition: iKinSlv.h:356
void printInfo(const std::string &typ, const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, const double t)
Definition: iKinSlv.cpp:1144
virtual bool handleJointsRestPosition(const yarp::os::Bottle *options, yarp::os::Bottle *reply=NULL)
Definition: iKinSlv.cpp:1195
void latchUncontrolledJoints(yarp::sig::Vector &joints)
Definition: iKinSlv.cpp:408
virtual void suspend()
Suspend the solver's main loop.
Definition: iKinSlv.cpp:1617
yarp::sig::Vector dof
Definition: iKinSlv.h:386
virtual ~CartesianSolver()
Default destructor.
Definition: iKinSlv.cpp:1749
virtual bool isClosed() const
To be called to check whether the solver has received a [quit] request.
Definition: iKinSlv.h:549
yarp::dev::PolyDriver * waitPart(const yarp::os::Property &partOpt)
Definition: iKinSlv.cpp:301
void getFeedback(const bool wait=false)
Definition: iKinSlv.cpp:425
virtual void close()
Stop the solver and dispose it.
Definition: iKinSlv.cpp:1532
virtual bool & getTimeoutFlag()
To be called to check whether communication timeout has been detected.
Definition: iKinSlv.h:559
void fillDOFInfo(yarp::os::Bottle &reply)
Definition: iKinSlv.cpp:507
yarp::sig::Vector w_2ndTask
Definition: iKinSlv.h:392
virtual void prepareJointsRestTask()
Definition: iKinSlv.cpp:1489
void send(const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok)
Definition: iKinSlv.cpp:1126
std::deque< yarp::dev::PolyDriver * > drv
Definition: iKinSlv.h:349
yarp::os::Port * rpcPort
Definition: iKinSlv.h:359
yarp::sig::Vector qd_3rdTask
Definition: iKinSlv.h:394
PartDescriptor * prt
Definition: iKinSlv.h:348
bool isNewDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1254
virtual bool threadInit()
Definition: iKinSlv.cpp:1595
yarp::sig::Vector get_xd()
Definition: iKinSlv.cpp:94
double * get_tokenPtr()
Definition: iKinSlv.h:302
bool handleTarget(yarp::os::Bottle *b)
Definition: iKinSlv.cpp:125
yarp::sig::Vector dof
Definition: iKinSlv.h:292
InputPort(CartesianSolver *_slv)
Definition: iKinSlv.cpp:60
CartesianSolver * slv
Definition: iKinSlv.h:280
bool handlePose(const int newPose)
Definition: iKinSlv.cpp:162
bool handleDOF(yarp::os::Bottle *b)
Definition: iKinSlv.cpp:141
yarp::sig::Vector get_dof()
Definition: iKinSlv.cpp:85
bool handleMode(const int newMode)
Definition: iKinSlv.cpp:186
std::mutex mtx
Definition: iKinSlv.h:282
void set_dof(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:77
virtual void onRead(yarp::os::Bottle &b)
Definition: iKinSlv.cpp:208
bool & get_contMode()
Definition: iKinSlv.h:301
yarp::sig::Vector xd
Definition: iKinSlv.h:293
void reset_xd(const yarp::sig::Vector &_xd)
Definition: iKinSlv.cpp:103
CartesianSolver * slv
Definition: iKinSlv.h:268
virtual bool read(yarp::os::ConnectionReader &connection)
RpcProcessor(CartesianSolver *_slv)
Definition: iKinSlv.h:273
CartesianSolver * slv
Definition: iKinSlv.h:319
virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q)
Defines the callback body to be called at each iteration.
Definition: iKinSlv.cpp:260
SolverCallback(CartesianSolver *_slv)
Definition: iKinSlv.h:322
Derived class which implements the on-line solver for the chain torso+arm.
Definition: iKinSlv.h:585
iCubArmCartesianSolver(const std::string &_slvName="armCartSolver")
Constructor.
Definition: iKinSlv.h:598
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)
Definition: iKinSlv.cpp:1761
virtual bool decodeDOF(const yarp::sig::Vector &_dof)
Definition: iKinSlv.cpp:1831
virtual bool open(yarp::os::Searchable &options)
Configure the solver and start it up.
Definition: iKinSlv.cpp:1813
Derived class which implements the on-line solver for the leg chain.
Definition: iKinSlv.h:611
virtual PartDescriptor * getPartDesc(yarp::os::Searchable &options)
Definition: iKinSlv.cpp:1857
iCubLegCartesianSolver(const std::string &_slvName="legCartSolver")
Constructor.
Definition: iKinSlv.h:623
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:355
Class for inverting chain's kinematics based on IpOpt lib.
Definition: iKinIpOpt.h:199
Class for defining iteration callback.
Definition: iKinIpOpt.h:44
A class for defining generic Limb.
Definition: iKinFwd.h:874
Class for defining Linear Inequality Constraints of the form lB <= C*q <= uB for the nonlinear proble...
Definition: iKinIpOpt.h:70
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
const FSC max
Definition: strain.h:48
const FSC min
Definition: strain.h:49
iKinLinIneqConstr * cns
Definition: iKinSlv.h:332
std::deque< bool > rvs
Definition: iKinSlv.h:334
std::deque< yarp::os::Property > prp
Definition: iKinSlv.h:333