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 
Class representing an external contact acting on a link of the robot body.
Definition: dynContact.h:52
virtual unsigned int getLinkNumber() const
Get the link number (where 0 is the first link of the chain).
Definition: dynContact.cpp:84
bool checkVectorDim(const yarp::sig::Vector &v, unsigned int dim, const std::string &descr="")
Definition: dynContact.cpp:274
unsigned int verbose
verbosity flag
Definition: dynContact.h:80
yarp::sig::Vector F
contact force
Definition: dynContact.h:68
virtual const yarp::sig::Vector & getForce() const
Get the contact force.
Definition: dynContact.cpp:74
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
virtual bool setForce(const yarp::sig::Vector &_F)
Set the contact force.
Definition: dynContact.cpp:101
virtual bool setForceDirection(const yarp::sig::Vector &_Fdir)
Set the direction of the contact force.
Definition: dynContact.cpp:122
yarp::sig::Vector Mu
contact moment
Definition: dynContact.h:72
bool muKnown
True if the moment applied at the contact point is known.
Definition: dynContact.h:75
virtual void unfixMoment()
Set the flag muKnown to false so that when estimating the contact wrenches the solver estimates also ...
Definition: dynContact.cpp:189
unsigned int linkNumber
number of the link where the contact is applied
Definition: dynContact.h:62
yarp::sig::Vector Fdir
contact force direction (unit vector)
Definition: dynContact.h:66
virtual void setBodyPart(BodyPart _bodyPart)
Set the body part of this contact.
Definition: dynContact.cpp:161
virtual bool setCoP(const yarp::sig::Vector &_CoP)
Set the contact center of pressure in link reference frame.
Definition: dynContact.cpp:150
virtual const yarp::sig::Vector & getMoment() const
Get the contact moment.
Definition: dynContact.cpp:80
virtual bool setForceModule(double _Fmodule)
Set the contact force module.
Definition: dynContact.cpp:111
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
virtual BodyPart getBodyPart() const
Get the body part of the contact.
Definition: dynContact.cpp:86
virtual unsigned long getId() const
Get the id of this contact.
Definition: dynContact.cpp:90
dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP, const yarp::sig::Vector &_Mu, const yarp::sig::Vector &_Fdir)
Constructor with known moment (usually zero) and known force direction.
virtual void unfixForceDirection()
Set the flag fDirKnown to false so that when estimating the contact wrenches the solver estimates als...
Definition: dynContact.cpp:187
virtual std::string getBodyPartName() const
Get the name of the contact body part.
Definition: dynContact.cpp:88
virtual const yarp::sig::Vector & getCoP() const
Get the contact center of pressure expressed in the link reference frame.
Definition: dynContact.cpp:82
static unsigned long ID
Definition: dynContact.h:55
virtual std::string toString(int precision=-1) const
Convert this contact into a string.
Definition: dynContact.cpp:263
virtual void setVerbose(unsigned int verb=VERBOSE)
Set the verbosity level of comments during operations.
Definition: dynContact.cpp:270
virtual void setLinkNumber(unsigned int _linkNum)
Set the contact link number (0 is the first link)
Definition: dynContact.cpp:157
virtual bool fixForceDirection(const yarp::sig::Vector &_Fdir)
Fix the direction of the contact force.
Definition: dynContact.cpp:167
virtual bool fixMoment(const yarp::sig::Vector &_Mu)
Fix the contact moment.
virtual bool fixMoment()
Equivalent to calling fixMoment(zeros(3)).
Definition: dynContact.cpp:175
yarp::sig::Vector CoP
center of pressure of the contact expressed w.r.t. the reference frame of the link
Definition: dynContact.h:64
virtual const yarp::sig::Vector & getForceDirection() const
Get the contact force direction.
Definition: dynContact.cpp:76
virtual yarp::sig::Vector getForceMoment() const
Get the contact force and moment in a single (6x1) vector.
Definition: dynContact.cpp:72
dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP)
Constructor with unknown moment and force direction.
BodyPart bodyPart
part of the body of the robot where the contact is applied
Definition: dynContact.h:60
virtual bool isForceDirectionKnown() const
Get true if the direction of the force applied at this contact is known a-priori.
Definition: dynContact.cpp:97
dynContact()
Default constructor.
Definition: dynContact.cpp:33
virtual bool isMomentKnown() const
Get true if the moment applied at this contact is known a-priori.
Definition: dynContact.cpp:95
virtual bool read(yarp::os::ConnectionReader &connection) override
Definition: dynContact.cpp:229
virtual double getForceModule() const
Get the contact force module.
Definition: dynContact.cpp:78
bool fDirKnown
True if the direction of the force applied at the contact point is known.
Definition: dynContact.h:77
dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP, const yarp::sig::Vector &_Mu)
Constructor with known moment (usually zero) and unknown force direction.
double Fmodule
contact force module
Definition: dynContact.h:70
virtual bool setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
Set the contact force and moment.
Definition: dynContact.cpp:139
virtual bool setMoment(const yarp::sig::Vector &_Mu)
Set the contact moment.
Definition: dynContact.cpp:132
static int v
Definition: iCub_Sim.cpp:42
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.