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 
223 #ifndef __IKINSLV_H__
224 #define __IKINSLV_H__
225 
226 #include <mutex>
227 #include <condition_variable>
228 #include <string>
229 #include <deque>
230 
231 #include <yarp/os/BufferedPort.h>
232 #include <yarp/os/PeriodicThread.h>
233 #include <yarp/sig/Vector.h>
234 #include <yarp/sig/Matrix.h>
235 
236 #include <yarp/dev/ControlBoardInterfaces.h>
237 #include <yarp/dev/PolyDriver.h>
238 
239 #include <iCub/iKin/iKinHlp.h>
240 #include <iCub/iKin/iKinIpOpt.h>
241 
242 
243 namespace iCub
244 {
245 
246 namespace iKin
247 {
248 
249 class CartesianSolver;
250 
251 class RpcProcessor : public yarp::os::PortReader
252 {
253 protected:
255 
256  virtual bool read(yarp::os::ConnectionReader &connection);
257 
258 public:
259  RpcProcessor(CartesianSolver *_slv) : slv(_slv) { }
260 };
261 
262 
263 class InputPort : public yarp::os::BufferedPort<yarp::os::Bottle>
264 {
265 protected:
267 
268  std::mutex mtx;
269 
270  bool contMode;
271  bool isNew;
272  size_t maxLen;
273  int pose;
274 
275  double token;
276  double *pToken;
277 
278  yarp::sig::Vector dof;
279  yarp::sig::Vector xd;
280 
281  virtual void onRead(yarp::os::Bottle &b);
282 
283 public:
284  InputPort(CartesianSolver *_slv);
285 
286  int &get_pose() { return pose; }
287  bool &get_contMode() { return contMode; }
288  double *get_tokenPtr() { return pToken; }
289  yarp::sig::Vector get_dof();
290  yarp::sig::Vector get_xd();
291 
292  void set_dof(const yarp::sig::Vector &_dof);
293  void reset_xd(const yarp::sig::Vector &_xd);
294  bool isNewDataEvent();
295  bool handleTarget(yarp::os::Bottle *b);
296  bool handleDOF(yarp::os::Bottle *b);
297  bool handlePose(const int newPose);
298  bool handleMode(const int newMode);
299 };
300 
301 
303 {
304 protected:
306 
307 public:
308  SolverCallback(CartesianSolver *_slv) : slv(_slv) { }
309 
310  virtual void exec(const yarp::sig::Vector &xd, const yarp::sig::Vector &q);
311 };
312 
313 
315 {
319  std::deque<yarp::os::Property> prp;
320  std::deque<bool> rvs;
321  int num;
322 };
323 
324 
330 class CartesianSolver : public yarp::os::PeriodicThread,
331  protected CartesianHelper
332 {
333 protected:
335  std::deque<yarp::dev::PolyDriver*> drv;
336  std::deque<yarp::dev::IControlLimits*> lim;
337  std::deque<yarp::dev::IEncoders*> enc;
338  std::deque<int> jnt;
339  std::deque<int*> rmp;
340 
343 
345  yarp::os::Port *rpcPort;
347  yarp::os::BufferedPort<yarp::os::Bottle> *outPort;
348  std::mutex mtx;
349 
350  std::string slvName;
351  std::string type;
352  unsigned int period;
353  unsigned int ctrlPose;
354  bool fullDOF;
357  bool closing;
358  bool closed;
360  bool verbosity;
365  double token;
366  double *pToken;
367 
368  yarp::sig::Matrix hwLimits;
369  yarp::sig::Matrix swLimits;
370 
371  yarp::sig::Vector unctrlJointsOld;
372  yarp::sig::Vector dof;
373 
374  yarp::sig::Vector restJntPos;
375  yarp::sig::Vector restWeights;
376 
377  yarp::sig::Vector xd_2ndTask;
378  yarp::sig::Vector w_2ndTask;
379 
380  yarp::sig::Vector qd_3rdTask;
381  yarp::sig::Vector w_3rdTask;
382  yarp::sig::Vector idx_3rdTask;
383 
384  std::mutex mtx_dofEvent;
385  std::condition_variable cv_dofEvent;
386 
387  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options)=0;
388  virtual yarp::sig::Vector solve(yarp::sig::Vector &xd);
389 
390  virtual yarp::sig::Vector &encodeDOF();
391  virtual bool decodeDOF(const yarp::sig::Vector &_dof);
392 
393  virtual bool handleJointsRestPosition(const yarp::os::Bottle *options,
394  yarp::os::Bottle *reply=NULL);
395  virtual bool handleJointsRestWeights(const yarp::os::Bottle *options,
396  yarp::os::Bottle *reply=NULL);
397 
398  yarp::dev::PolyDriver *waitPart(const yarp::os::Property &partOpt);
399 
400  bool isNewDOF(const yarp::sig::Vector &_dof);
401  bool changeDOF(const yarp::sig::Vector &_dof);
402 
403  bool alignJointsBounds();
404  bool setLimits(int axis, double min, double max);
405  void countUncontrolledJoints();
406  void latchUncontrolledJoints(yarp::sig::Vector &joints);
407  void getFeedback(const bool wait=false);
408  void initPos();
409  void lock();
410  void unlock();
411 
412  void waitDOFHandling();
413  void postDOFHandling();
414  void fillDOFInfo(yarp::os::Bottle &reply);
415  void send(const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok);
416  void printInfo(const std::string &typ, const yarp::sig::Vector &xd, const yarp::sig::Vector &x,
417  const yarp::sig::Vector &q, const double t);
418 
419  virtual void prepareJointsRestTask();
420  virtual void respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply);
421  virtual bool threadInit();
422  virtual void afterStart(bool);
423  virtual void run();
424  virtual void threadRelease();
425 
426  friend class RpcProcessor;
427  friend class InputPort;
428  friend class SolverCallback;
429 
430 public:
445  CartesianSolver(const std::string &_slvName);
446 
517  virtual bool open(yarp::os::Searchable &options);
518 
523  virtual void interrupt();
524 
528  virtual void close();
529 
535  virtual bool isClosed() const { return closed; }
536 
545  virtual bool &getTimeoutFlag() { return timeout_detected; }
546 
550  virtual void suspend();
551 
555  virtual void resume();
556 
560  virtual ~CartesianSolver();
561 };
562 
563 
571 {
572 protected:
573  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
574  virtual bool decodeDOF(const yarp::sig::Vector &_dof);
575 
576 public:
584  iCubArmCartesianSolver(const std::string &_slvName="armCartSolver") : CartesianSolver(_slvName) { }
585 
586  virtual bool open(yarp::os::Searchable &options);
587 };
588 
589 
597 {
598 protected:
599  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
600 
601 public:
609  iCubLegCartesianSolver(const std::string &_slvName="legCartSolver") : CartesianSolver(_slvName) { }
610 };
611 
612 }
613 
614 }
615 
616 #endif
617 
618 
619 
yarp::sig::Vector idx_3rdTask
Definition: iKinSlv.h:382
std::deque< yarp::dev::IEncoders * > enc
Definition: iKinSlv.h:337
iCubArmCartesianSolver(const std::string &_slvName="armCartSolver")
Constructor.
Definition: iKinSlv.h:584
CartesianSolver * slv
Definition: iKinSlv.h:266
SolverCallback * clb
Definition: iKinSlv.h:342
std::deque< yarp::os::Property > prp
Definition: iKinSlv.h:319
iKinIpOptMin * slv
Definition: iKinSlv.h:341
yarp::os::Port * rpcPort
Definition: iKinSlv.h:345
double * get_tokenPtr()
Definition: iKinSlv.h:288
yarp::sig::Vector xd
Definition: iKinSlv.h:279
std::deque< yarp::dev::IControlLimits * > lim
Definition: iKinSlv.h:336
RpcProcessor * cmdProcessor
Definition: iKinSlv.h:344
std::deque< bool > rvs
Definition: iKinSlv.h:320
SolverCallback(CartesianSolver *_slv)
Definition: iKinSlv.h:308
PartDescriptor * prt
Definition: iKinSlv.h:334
bool & get_contMode()
Definition: iKinSlv.h:287
A Base class for defining a Serial Link Chain.
Definition: iKinFwd.h:354
Matrix alignJointsBounds(iKinChain *chain, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData)
Definition: utils.cpp:539
const FSC max
Definition: strain.h:48
Class for defining Linear Inequality Constraints of the form lB <= C*q <= uB for the nonlinear proble...
Definition: iKinIpOpt.h:69
iCubLegCartesianSolver(const std::string &_slvName="legCartSolver")
Constructor.
Definition: iKinSlv.h:609
const FSC min
Definition: strain.h:49
yarp::sig::Vector w_3rdTask
Definition: iKinSlv.h:381
Helper class providing useful methods to deal with Cartesian Solver options.
Definition: iKinHlp.h:47
yarp::sig::Vector unctrlJointsOld
Definition: iKinSlv.h:371
CartesianSolver * slv
Definition: iKinSlv.h:254
yarp::sig::Vector w_2ndTask
Definition: iKinSlv.h:378
Class for defining iteration callback.
Definition: iKinIpOpt.h:43
yarp::sig::Vector qd_3rdTask
Definition: iKinSlv.h:380
std::deque< int > jnt
Definition: iKinSlv.h:338
yarp::sig::Matrix hwLimits
Definition: iKinSlv.h:368
CartesianSolver * slv
Definition: iKinSlv.h:305
A class for defining generic Limb.
Definition: iKinFwd.h:873
yarp::sig::Matrix swLimits
Definition: iKinSlv.h:369
iKinLinIneqConstr * cns
Definition: iKinSlv.h:318
virtual bool & getTimeoutFlag()
To be called to check whether communication timeout has been detected.
Definition: iKinSlv.h:545
RpcProcessor(CartesianSolver *_slv)
Definition: iKinSlv.h:259
yarp::os::BufferedPort< yarp::os::Bottle > * outPort
Definition: iKinSlv.h:347
yarp::sig::Vector restWeights
Definition: iKinSlv.h:375
Derived class which implements the on-line solver for the leg chain.
Definition: iKinSlv.h:596
std::condition_variable cv_dofEvent
Definition: iKinSlv.h:385
virtual bool isClosed() const
To be called to check whether the solver has received a [quit] request.
Definition: iKinSlv.h:535
std::deque< yarp::dev::PolyDriver * > drv
Definition: iKinSlv.h:335
yarp::sig::Vector dof
Definition: iKinSlv.h:372
yarp::sig::Vector xd_2ndTask
Definition: iKinSlv.h:377
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
yarp::sig::Vector dof
Definition: iKinSlv.h:278
std::mutex mtx
Definition: iKinSlv.h:268
Derived class which implements the on-line solver for the chain torso+arm.
Definition: iKinSlv.h:570
virtual bool read(yarp::os::ConnectionReader &connection)
Class for inverting chain&#39;s kinematics based on IpOpt lib.
Definition: iKinIpOpt.h:198
yarp::sig::Vector restJntPos
Definition: iKinSlv.h:374
std::deque< int * > rmp
Definition: iKinSlv.h:339
Abstract class defining the core of on-line solvers.
Definition: iKinSlv.h:330
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData, double *timeStamp=nullptr)
Definition: utils.cpp:641