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 <string>
227 #include <deque>
228 
229 #include <yarp/os/BufferedPort.h>
230 #include <yarp/os/PeriodicThread.h>
231 #include <yarp/os/Mutex.h>
232 #include <yarp/os/Event.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  yarp::os::Mutex mutex;
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  yarp::os::Mutex mutex;
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  yarp::os::Event dofEvent;
385 
386  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options)=0;
387  virtual yarp::sig::Vector solve(yarp::sig::Vector &xd);
388 
389  virtual yarp::sig::Vector &encodeDOF();
390  virtual bool decodeDOF(const yarp::sig::Vector &_dof);
391 
392  virtual bool handleJointsRestPosition(const yarp::os::Bottle *options,
393  yarp::os::Bottle *reply=NULL);
394  virtual bool handleJointsRestWeights(const yarp::os::Bottle *options,
395  yarp::os::Bottle *reply=NULL);
396 
397  yarp::dev::PolyDriver *waitPart(const yarp::os::Property &partOpt);
398 
399  bool isNewDOF(const yarp::sig::Vector &_dof);
400  bool changeDOF(const yarp::sig::Vector &_dof);
401 
402  bool alignJointsBounds();
403  bool setLimits(int axis, double min, double max);
404  void countUncontrolledJoints();
405  void latchUncontrolledJoints(yarp::sig::Vector &joints);
406  void getFeedback(const bool wait=false);
407  void initPos();
408  void lock();
409  void unlock();
410 
411  void waitDOFHandling();
412  void postDOFHandling();
413  void fillDOFInfo(yarp::os::Bottle &reply);
414  void send(const yarp::sig::Vector &xd, const yarp::sig::Vector &x, const yarp::sig::Vector &q, double *tok);
415  void printInfo(const std::string &typ, const yarp::sig::Vector &xd, const yarp::sig::Vector &x,
416  const yarp::sig::Vector &q, const double t);
417 
418  virtual void prepareJointsRestTask();
419  virtual void respond(const yarp::os::Bottle &command, yarp::os::Bottle &reply);
420  virtual bool threadInit();
421  virtual void afterStart(bool);
422  virtual void run();
423  virtual void threadRelease();
424 
425  friend class RpcProcessor;
426  friend class InputPort;
427  friend class SolverCallback;
428 
429 public:
444  CartesianSolver(const std::string &_slvName);
445 
516  virtual bool open(yarp::os::Searchable &options);
517 
522  virtual void interrupt();
523 
527  virtual void close();
528 
534  virtual bool isClosed() const { return closed; }
535 
544  virtual bool &getTimeoutFlag() { return timeout_detected; }
545 
549  virtual void suspend();
550 
554  virtual void resume();
555 
559  virtual ~CartesianSolver();
560 };
561 
562 
570 {
571 protected:
572  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
573  virtual bool decodeDOF(const yarp::sig::Vector &_dof);
574 
575 public:
583  iCubArmCartesianSolver(const std::string &_slvName="armCartSolver") : CartesianSolver(_slvName) { }
584 
585  virtual bool open(yarp::os::Searchable &options);
586 };
587 
588 
596 {
597 protected:
598  virtual PartDescriptor *getPartDesc(yarp::os::Searchable &options);
599 
600 public:
608  iCubLegCartesianSolver(const std::string &_slvName="legCartSolver") : CartesianSolver(_slvName) { }
609 };
610 
611 }
612 
613 }
614 
615 #endif
616 
617 
618 
yarp::sig::Vector idx_3rdTask
Definition: iKinSlv.h:382
yarp::os::Event dofEvent
Definition: iKinSlv.h:384
std::deque< yarp::dev::IEncoders * > enc
Definition: iKinSlv.h:337
iCubArmCartesianSolver(const std::string &_slvName="armCartSolver")
Constructor.
Definition: iKinSlv.h:583
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::Mutex mutex
Definition: iKinSlv.h:268
yarp::os::Port * rpcPort
Definition: iKinSlv.h:345
bool getFeedback(Vector &fbTorso, Vector &fbHead, PolyDriver *drvTorso, PolyDriver *drvHead, const ExchangeData *commData, double *timeStamp=NULL)
Definition: utils.cpp:650
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:548
yarp::os::Mutex mutex
Definition: iKinSlv.h:348
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:608
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:544
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:595
virtual bool isClosed() const
To be called to check whether the solver has received a [quit] request.
Definition: iKinSlv.h:534
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
Derived class which implements the on-line solver for the chain torso+arm.
Definition: iKinSlv.h:569
virtual bool read(yarp::os::ConnectionReader &connection)
Class for inverting chain&#39;s kinematics based on IpOpt lib.
Definition: iKinIpOpt.h:197
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