iCub-main
dynContact.cpp
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 
8 #include <iostream>
9 #include <sstream>
10 #include <iomanip>
11 #include <string>
12 #include "stdio.h"
13 
14 #include <yarp/os/ConnectionReader.h>
15 #include <yarp/os/ConnectionWriter.h>
16 #include <yarp/math/Math.h>
18 #include <iCub/ctrl/math.h>
19 
20 using namespace std;
21 using namespace yarp::os;
22 using namespace yarp::sig;
23 using namespace yarp::math;
24 using namespace iCub::ctrl;
25 using namespace iCub::skinDynLib;
26 
27 
28 //~~~~~~~~~~~~~~~~~~~~~~
29 // DYN CONTACT
30 //~~~~~~~~~~~~~~~~~~~~~~
31 unsigned long dynContact::ID = 1;
32 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
33 dynContact::dynContact(){
34  init(BODY_PART_UNKNOWN, 0, zeros(3));
35 }
36 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
37 dynContact::dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const Vector &_CoP){
38  init(_bodyPart, _linkNumber, _CoP);
39 }
40 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
41 dynContact::dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const Vector &_CoP, const Vector &_Mu){
42  init(_bodyPart, _linkNumber, _CoP, _Mu);
43 }
44 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
45 dynContact::dynContact(const BodyPart &_bodyPart, unsigned int _linkNumber, const Vector &_CoP, const Vector &_Mu, const Vector &_Fdir){
46  init(_bodyPart, _linkNumber, _CoP, _Mu, _Fdir);
47 }
48 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
49 void dynContact::init(const BodyPart &_bodyPart, unsigned int _linkNumber, const Vector &_CoP, const Vector &_Mu, const Vector &_Fdir){
50  contactId = ID++;
51  setBodyPart(_bodyPart);
52  setLinkNumber(_linkNumber);
53  setCoP(_CoP);
54  Mu.resize(3, 0.0);
55  Fdir.resize(3, 0.0);
56  F.resize(3, 0.0);
57  Fmodule = 0.0;
58 
59  if(_Mu.size()==0)
60  muKnown = false;
61  else
62  fixMoment(_Mu);
63 
64  if(_Fdir.size()==0)
65  fDirKnown = false;
66  else
67  fixForceDirection(_Fdir);
68 }
69 //~~~~~~~~~~~~~~~~~~~~~~
70 // GET methods
71 //~~~~~~~~~~~~~~~~~~~~~~
72 Vector dynContact::getForceMoment() const{ return cat(F, Mu); }
73 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
74 const Vector& dynContact::getForce() const{ return F;}
75 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
76 const Vector& dynContact::getForceDirection() const{ return Fdir;}
77 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
78 double dynContact::getForceModule() const{ return Fmodule;}
79 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
80 const Vector& dynContact::getMoment() const{ return Mu;}
81 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
82 const Vector& dynContact::getCoP() const{ return CoP;}
83 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
84 unsigned int dynContact::getLinkNumber() const{ return linkNumber;}
85 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
86 BodyPart dynContact::getBodyPart() const{ return bodyPart;}
87 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
88 string dynContact::getBodyPartName() const{ return BodyPart_s[bodyPart];}
89 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
90 unsigned long dynContact::getId() const{ return contactId;}
91 
92 //~~~~~~~~~~~~~~~~~~~~~~
93 // IS methods
94 //~~~~~~~~~~~~~~~~~~~~~~
95 bool dynContact::isMomentKnown() const{ return muKnown;}
96 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
97 bool dynContact::isForceDirectionKnown() const{ return fDirKnown;}
98 //~~~~~~~~~~~~~~~~~~~~~~
99 // SET methods
100 //~~~~~~~~~~~~~~~~~~~~~~
101 bool dynContact::setForce(const Vector &_F){
102  if(!checkVectorDim(_F, 3, "force"))
103  return false;
104  F = _F;
105  Fmodule = norm(_F);
106  if(Fmodule!=0.0)
107  Fdir = _F / Fmodule;
108  return true;
109 }
110 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
111 bool dynContact::setForceModule(double _Fmodule){
112  if(_Fmodule<0){
113  if(verbose)
114  fprintf(stderr, "Error in dynContact: negative force module, %f\n", _Fmodule);
115  return false;
116  }
117  Fmodule = _Fmodule;
118  F=Fmodule*Fdir;
119  return true;
120 }
121 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
122 bool dynContact::setForceDirection(const Vector &_Fdir){
123  if(!checkVectorDim(_Fdir, 3, "force direction"))
124  return false;
125  double FdirNorm = norm(_Fdir);
126  if(FdirNorm != 0.0)
127  Fdir = _Fdir / FdirNorm;
128  F=Fmodule*Fdir;
129  return true;
130 }
131 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
132 bool dynContact::setMoment(const Vector &_Mu){
133  if(!checkVectorDim(_Mu, 3, "moment"))
134  return false;
135  Mu = _Mu;
136  return true;
137 }
138 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
139 bool dynContact::setForceMoment(const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu){
140  return setForce(_F) && setMoment(_Mu);
141 }
142 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
143 bool dynContact::setForceMoment(const yarp::sig::Vector &_FMu){
144  if(!checkVectorDim(_FMu, 6, "force moment"))
145  return false;
146  bool res = setForce(_FMu.subVector(0,2));
147  return res && setMoment(_FMu.subVector(3,5));
148 }
149 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
150 bool dynContact::setCoP(const Vector &_CoP){
151  if(!checkVectorDim(_CoP, 3, "Center of pressure"))
152  return false;
153  CoP = _CoP;
154  return true;
155 }
156 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
157 void dynContact::setLinkNumber(unsigned int _linkNum){
158  linkNumber = _linkNum;
159 }
160 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
161 void dynContact::setBodyPart(BodyPart _bodyPart){
162  bodyPart = _bodyPart;
163 }
164 //~~~~~~~~~~~~~~~~~~~~~~
165 // FIX/UNFIX methods
166 //~~~~~~~~~~~~~~~~~~~~~~
167 bool dynContact::fixForceDirection(const Vector &_Fdir){
168  if(setForceDirection(_Fdir)){
169  fDirKnown = true;
170  return true;
171  }
172  return false;
173 }
174 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
175 bool dynContact::fixMoment(){
176  return fixMoment(zeros(3));
177 }
178 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
179 bool dynContact::fixMoment(const Vector &_Mu){
180  if(setMoment(_Mu)){
181  muKnown = true;
182  return true;
183  }
184  return false;
185 }
186 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
187 void dynContact::unfixForceDirection(){ fDirKnown=false;}
188 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
189 void dynContact::unfixMoment(){ muKnown=false;}
190 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
191 //~~~~~~~~~~~~~~~~~~~~~~
192 // SERIALIZATION methods
193 //~~~~~~~~~~~~~~~~~~~~~~
194 bool dynContact::write(ConnectionWriter& connection) const{
195  // represent a dynContact as a list of 4 elements that are:
196  // - a list of 3 int, i.e. contactId, bodyPart, linkNumber
197  // - a list of 3 double, i.e. the CoP
198  // - a list of 3 double, i.e. the force
199  // - a list of 3 double, i.e. the moment
200 
201  connection.appendInt32(BOTTLE_TAG_LIST);
202  connection.appendInt32(4);
203  // list of 3 int, i.e. contactId, bodyPart, linkNumber
204  connection.appendInt32(BOTTLE_TAG_LIST + BOTTLE_TAG_INT32);
205  connection.appendInt32(3);
206  connection.appendInt32(contactId);
207  connection.appendInt32(bodyPart); // left_arm, right_arm, ...
208  connection.appendInt32(linkNumber);
209  // list of 3 double, i.e. the CoP
210  connection.appendInt32(BOTTLE_TAG_LIST + BOTTLE_TAG_FLOAT64);
211  connection.appendInt32(3);
212  for(int i=0;i<3;i++) connection.appendFloat64(CoP[i]);
213  // list of 3 double, i.e. the force
214  connection.appendInt32(BOTTLE_TAG_LIST + BOTTLE_TAG_FLOAT64);
215  connection.appendInt32(3);
216  for(int i=0;i<3;i++) connection.appendFloat64(F[i]);
217  // list of 3 double, i.e. the moment
218  connection.appendInt32(BOTTLE_TAG_LIST + BOTTLE_TAG_FLOAT64);
219  connection.appendInt32(3);
220  for(int i=0;i<3;i++) connection.appendFloat64(Mu[i]);
221 
222  // if someone is foolish enough to connect in text mode,
223  // let them see something readable.
224  connection.convertTextMode();
225 
226  return !connection.isError();
227 }
228 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
229 bool dynContact::read(ConnectionReader& connection){
230  // auto-convert text mode interaction
231  connection.convertTextMode();
232 
233  // represent a dynContact as a list of 4 elements that are:
234  // - a list of 3 int, i.e. contactId, bodyPart, linkNumber
235  // - a list of 3 double, i.e. the CoP
236  // - a list of 3 double, i.e. the force
237  // - a list of 3 double, i.e. the moment
238  if(connection.expectInt32()!= BOTTLE_TAG_LIST || connection.expectInt32()!=4)
239  return false;
240  // - a list of 3 int, i.e. contactId, bodyPart, linkNumber
241  if(connection.expectInt32()!=BOTTLE_TAG_LIST+BOTTLE_TAG_INT32 || connection.expectInt32()!=3)
242  return false;
243  contactId = connection.expectInt32();
244  bodyPart = (BodyPart)connection.expectInt32();
245  linkNumber = connection.expectInt32();
246  // - a list of 3 double, i.e. the CoP
247  if(connection.expectInt32()!=BOTTLE_TAG_LIST+BOTTLE_TAG_FLOAT64 || connection.expectInt32()!=3)
248  return false;
249  for(int i=0;i<3;i++) CoP[i] = connection.expectFloat64();
250  // - a list of 3 double, i.e. the force
251  if(connection.expectInt32()!=BOTTLE_TAG_LIST+BOTTLE_TAG_FLOAT64 || connection.expectInt32()!=3)
252  return false;
253  for(int i=0;i<3;i++) F[i] = connection.expectFloat64();
254  setForce(F);
255  // - a list of 3 double, i.e. the moment
256  if(connection.expectInt32()!=BOTTLE_TAG_LIST+BOTTLE_TAG_FLOAT64 || connection.expectInt32()!=3)
257  return false;
258  for(int i=0;i<3;i++) Mu[i] = connection.expectFloat64();
259 
260  return !connection.isError();
261 }
262 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
263 string dynContact::toString(int precision) const{
264  stringstream res;
265  res<< "Contact id: "<< contactId<< ", Body part: "<< BodyPart_s[bodyPart]<< ", link: "<< linkNumber<< ", CoP: "<<
266  CoP.toString(precision)<< ", F: "<< F.toString(precision)<< ", M: "<< Mu.toString(precision);
267  return res.str();
268 }
269 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
270 void dynContact::setVerbose(unsigned int verb){
271  verbose = verb;
272 }
273 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
274 bool dynContact::checkVectorDim(const Vector &v, unsigned int dim, const string &descr){
275  if(v.length() != dim){
276  if(verbose)
277  fprintf(stderr, "Error in dynContact: unexpected dimension of vector %s, %d\n", descr.c_str(), (int)v.length());
278  return false;
279  }
280  return true;
281 }
282 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
283 
284 
iCub::skinDynLib::BODY_PART_UNKNOWN
@ BODY_PART_UNKNOWN
Definition: common.h:45
iCub::skinDynLib
Definition: common.h:35
dynContact.h
strain::regulation::write
bool write(const std::string filename, const FullRegulation &reg)
F
F
Definition: compute_ekf_fast.m:22
math.h
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:199
iCub::ctrl
Definition: adaptWinPolyEstimator.h:37
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
v
static int v
Definition: iCub_Sim.cpp:42
iCub::skinDynLib::getBodyPart
BodyPart getBodyPart(SkinPart s)
Definition: common.cpp:26
iCub::skinDynLib::BodyPart_s
const std::string BodyPart_s[]
Definition: common.h:49
fprintf
fprintf(fid,'\n')
zeros
zeros(2, 2) eye(2
eth::parser::read
bool read(yarp::os::Searchable &cfgtotal, pc104Data &pc104data)
Definition: ethParser.cpp:92