iCub-main
dynContact.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2010-2011 RobotCub Consortium
3  * Author: Andrea Del Prete
4  * CopyPolicy: Released under the terms of the GNU GPL v2.0.
5  *
6  */
7 
25 #ifndef __DINCONT_H__
26 #define __DINCONT_H__
27 
28 #include <yarp/os/Portable.h>
29 #include <yarp/sig/Vector.h>
30 #include <yarp/sig/Matrix.h>
31 #include "iCub/skinDynLib/common.h"
32 
33 namespace iCub
34 {
35 namespace skinDynLib
36 {
37 
51 class dynContact : public yarp::os::Portable
52 {
53 protected:
54  // static variable containing the id of the last contact created
55  static unsigned long ID;
56  // unique id of the contact
57  unsigned long contactId;
58 
62  unsigned int linkNumber;
64  yarp::sig::Vector CoP;
66  yarp::sig::Vector Fdir;
68  yarp::sig::Vector F;
70  double Fmodule;
72  yarp::sig::Vector Mu;
73 
75  bool muKnown;
77  bool fDirKnown;
78 
80  unsigned int verbose;
81 
82 
83  void init(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
84  const yarp::sig::Vector &_Mu=yarp::sig::Vector(0), const yarp::sig::Vector &_Fdir=yarp::sig::Vector(0));
85  bool checkVectorDim(const yarp::sig::Vector &v, unsigned int dim, const std::string &descr="");
86 
87 public:
88  //~~~~~~~~~~~~~~~~~~~~~~
89  // CONSTRUCTORS
90  //~~~~~~~~~~~~~~~~~~~~~~
94  dynContact();
101  dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP);
109  dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP, const yarp::sig::Vector &_Mu);
118  dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
119  const yarp::sig::Vector &_Mu, const yarp::sig::Vector &_Fdir);
120 
121  //~~~~~~~~~~~~~~~~~~~~~~
122  // GET methods
123  //~~~~~~~~~~~~~~~~~~~~~~
128  virtual yarp::sig::Vector getForceMoment() const;
133  virtual const yarp::sig::Vector& getForce() const;
138  virtual const yarp::sig::Vector& getForceDirection() const;
143  virtual double getForceModule() const;
148  virtual const yarp::sig::Vector& getMoment() const;
153  virtual const yarp::sig::Vector& getCoP() const;
158  virtual unsigned int getLinkNumber() const;
163  virtual BodyPart getBodyPart() const;
168  virtual std::string getBodyPartName() const;
173  virtual unsigned long getId() const;
174 
175  //~~~~~~~~~~~~~~~~~~~~~~
176  // IS methods
177  //~~~~~~~~~~~~~~~~~~~~~~
182  virtual bool isMomentKnown() const;
187  virtual bool isForceDirectionKnown() const;
188 
189  //~~~~~~~~~~~~~~~~~~~~~~
190  // SET methods
191  //~~~~~~~~~~~~~~~~~~~~~~
197  virtual bool setForce(const yarp::sig::Vector &_F);
203  virtual bool setForceModule(double _Fmodule);
210  virtual bool setForceDirection(const yarp::sig::Vector &_Fdir);
216  virtual bool setMoment(const yarp::sig::Vector &_Mu);
223  virtual bool setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu);
229  virtual bool setForceMoment(const yarp::sig::Vector &_FMu);
235  virtual bool setCoP(const yarp::sig::Vector &_CoP);
241  virtual void setLinkNumber(unsigned int _linkNum);
247  virtual void setBodyPart(BodyPart _bodyPart);
248 
249  //~~~~~~~~~~~~~~~~~~~~~~
250  // FIX/UNFIX methods
251  //~~~~~~~~~~~~~~~~~~~~~~
260  virtual bool fixForceDirection(const yarp::sig::Vector &_Fdir);
264  virtual bool fixMoment();
273  virtual bool fixMoment(const yarp::sig::Vector &_Mu);
279  virtual void unfixForceDirection();
285  virtual void unfixMoment();
286 
287  //~~~~~~~~~~~~~~~~~~~~~~
288  // SERIALIZATION methods
289  //~~~~~~~~~~~~~~~~~~~~~~
290  /*
291  * Read dynContact from a connection. It expects a list of 4 elements, that are:
292  * - a list of 3 int, containing contactId, bodyPart and linkNumber
293  * - a list of 3 double, containing the CoP
294  * - a list of 3 double, containing the force
295  * - a list of 3 double, containing the moment
296  * @param connection the connection to read from
297  * @return true iff a dynContact was read correctly
298  */
299  virtual bool read(yarp::os::ConnectionReader& connection) override;
309  virtual bool write(yarp::os::ConnectionWriter& connection) const override;
310 
311 
319  virtual std::string toString(int precision=-1) const;
324  virtual void setVerbose(unsigned int verb = VERBOSE);
325 };
326 
327 }
328 
329 }
330 #endif
331 
332 
iCub::skinDynLib::dynContact::setForceDirection
virtual bool setForceDirection(const yarp::sig::Vector &_Fdir)
Set the direction of the contact force.
Definition: dynContact.cpp:122
iCub
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.
Definition: emotionInterface.h:17
iCub::skinDynLib::dynContact::setVerbose
virtual void setVerbose(unsigned int verb=VERBOSE)
Set the verbosity level of comments during operations.
Definition: dynContact.cpp:270
iCub::skinDynLib::dynContact::getForceModule
virtual double getForceModule() const
Get the contact force module.
Definition: dynContact.cpp:78
iCub::skinDynLib::dynContact::dynContact
dynContact()
Default constructor.
Definition: dynContact.cpp:33
iCub::skinDynLib::dynContact::contactId
unsigned long contactId
Definition: dynContact.h:57
iCub::skinDynLib::dynContact::write
virtual bool write(yarp::os::ConnectionWriter &connection) const override
Write dynContact to a connection as a list of 4 elements, that are:
Definition: dynContact.cpp:194
iCub::skinDynLib::dynContact::unfixForceDirection
virtual void unfixForceDirection()
Set the flag fDirKnown to false so that when estimating the contact wrenches the solver estimates als...
Definition: dynContact.cpp:187
iCub::skinDynLib::dynContact::toString
virtual std::string toString(int precision=-1) const
Convert this contact into a string.
Definition: dynContact.cpp:263
iCub::skinDynLib::dynContact::fixMoment
virtual bool fixMoment()
Equivalent to calling fixMoment(zeros(3)).
Definition: dynContact.cpp:175
iCub::skinDynLib::dynContact::isForceDirectionKnown
virtual bool isForceDirectionKnown() const
Get true if the direction of the force applied at this contact is known a-priori.
Definition: dynContact.cpp:97
iCub::skinDynLib::dynContact::getBodyPart
virtual BodyPart getBodyPart() const
Get the body part of the contact.
Definition: dynContact.cpp:86
iCub::skinDynLib::dynContact::getForceMoment
virtual yarp::sig::Vector getForceMoment() const
Get the contact force and moment in a single (6x1) vector.
Definition: dynContact.cpp:72
iCub::skinDynLib::dynContact::getCoP
virtual const yarp::sig::Vector & getCoP() const
Get the contact center of pressure expressed in the link reference frame.
Definition: dynContact.cpp:82
iCub::skinDynLib::dynContact::fixForceDirection
virtual bool fixForceDirection(const yarp::sig::Vector &_Fdir)
Fix the direction of the contact force.
Definition: dynContact.cpp:167
iCub::skinDynLib::dynContact::getForceDirection
virtual const yarp::sig::Vector & getForceDirection() const
Get the contact force direction.
Definition: dynContact.cpp:76
iCub::skinDynLib::dynContact::setBodyPart
virtual void setBodyPart(BodyPart _bodyPart)
Set the body part of this contact.
Definition: dynContact.cpp:161
iCub::skinDynLib::dynContact::getId
virtual unsigned long getId() const
Get the id of this contact.
Definition: dynContact.cpp:90
iCub::skinDynLib::dynContact::muKnown
bool muKnown
True if the moment applied at the contact point is known.
Definition: dynContact.h:75
iCub::skinDynLib::dynContact::setCoP
virtual bool setCoP(const yarp::sig::Vector &_CoP)
Set the contact center of pressure in link reference frame.
Definition: dynContact.cpp:150
iCub::skinDynLib::dynContact::isMomentKnown
virtual bool isMomentKnown() const
Get true if the moment applied at this contact is known a-priori.
Definition: dynContact.cpp:95
iCub::skinDynLib::dynContact::init
void init(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP, const yarp::sig::Vector &_Mu=yarp::sig::Vector(0), const yarp::sig::Vector &_Fdir=yarp::sig::Vector(0))
Definition: dynContact.cpp:49
iCub::skinDynLib::dynContact::getMoment
virtual const yarp::sig::Vector & getMoment() const
Get the contact moment.
Definition: dynContact.cpp:80
iCub::skinDynLib::dynContact::Fdir
yarp::sig::Vector Fdir
contact force direction (unit vector)
Definition: dynContact.h:66
iCub::skinDynLib::dynContact::F
yarp::sig::Vector F
contact force
Definition: dynContact.h:68
iCub::skinDynLib::dynContact::fDirKnown
bool fDirKnown
True if the direction of the force applied at the contact point is known.
Definition: dynContact.h:77
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::skinDynLib::VERBOSE
@ VERBOSE
Definition: common.h:38
iCub::skinDynLib::dynContact::read
virtual bool read(yarp::os::ConnectionReader &connection) override
Definition: dynContact.cpp:229
iCub::skinDynLib::dynContact::verbose
unsigned int verbose
verbosity flag
Definition: dynContact.h:80
iCub::skinDynLib::dynContact::getLinkNumber
virtual unsigned int getLinkNumber() const
Get the link number (where 0 is the first link of the chain).
Definition: dynContact.cpp:84
iCub::skinDynLib::dynContact::Fmodule
double Fmodule
contact force module
Definition: dynContact.h:70
iCub::skinDynLib::dynContact::bodyPart
BodyPart bodyPart
part of the body of the robot where the contact is applied
Definition: dynContact.h:60
iCub::skinDynLib::dynContact::ID
static unsigned long ID
Definition: dynContact.h:55
iCub::skinDynLib::dynContact::getBodyPartName
virtual std::string getBodyPartName() const
Get the name of the contact body part.
Definition: dynContact.cpp:88
common.h
iCub::skinDynLib::dynContact::checkVectorDim
bool checkVectorDim(const yarp::sig::Vector &v, unsigned int dim, const std::string &descr="")
Definition: dynContact.cpp:274
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
iCub::skinDynLib::dynContact::setLinkNumber
virtual void setLinkNumber(unsigned int _linkNum)
Set the contact link number (0 is the first link)
Definition: dynContact.cpp:157
iCub::skinDynLib::dynContact
Definition: dynContact.h:51
v
static int v
Definition: iCub_Sim.cpp:42
iCub::skinDynLib::dynContact::Mu
yarp::sig::Vector Mu
contact moment
Definition: dynContact.h:72
iCub::skinDynLib::dynContact::unfixMoment
virtual void unfixMoment()
Set the flag muKnown to false so that when estimating the contact wrenches the solver estimates also ...
Definition: dynContact.cpp:189
iCub::skinDynLib::dynContact::linkNumber
unsigned int linkNumber
number of the link where the contact is applied
Definition: dynContact.h:62
iCub::skinDynLib::dynContact::setForceMoment
virtual bool setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the contact force and moment.
Definition: dynContact.cpp:139
iCub::skinDynLib::dynContact::setForce
virtual bool setForce(const yarp::sig::Vector &_F)
Set the contact force.
Definition: dynContact.cpp:101
iCub::skinDynLib::dynContact::getForce
virtual const yarp::sig::Vector & getForce() const
Get the contact force.
Definition: dynContact.cpp:74
iCub::skinDynLib::dynContact::setMoment
virtual bool setMoment(const yarp::sig::Vector &_Mu)
Set the contact moment.
Definition: dynContact.cpp:132
iCub::skinDynLib::dynContact::CoP
yarp::sig::Vector CoP
center of pressure of the contact expressed w.r.t. the reference frame of the link
Definition: dynContact.h:64
iCub::skinDynLib::dynContact::setForceModule
virtual bool setForceModule(double _Fmodule)
Set the contact force module.
Definition: dynContact.cpp:111