iCub-main
skinContact.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 <yarp/os/ConnectionReader.h>
12 #include <yarp/os/ConnectionWriter.h>
13 #include <yarp/math/Math.h>
15 
16 using namespace iCub::skinDynLib;
17 using namespace yarp::math;
18 using namespace yarp::sig;
19 using namespace yarp::os;
20 using namespace std;
21 
22 //~~~~~~~~~~~~~~~~~~~~~~
23 // CONSTRUCTORS
24 //~~~~~~~~~~~~~~~~~~~~~~
26  :dynContact(c), skinPart(SKIN_PART_UNKNOWN), geoCenter(zeros(3)), pressure(0.0), activeTaxels(0), normalDir(zeros(3)) {}
27 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
28 skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
29  const yarp::sig::Vector &_geoCenter, unsigned int _activeTaxels, double _pressure)
30 :dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
31 geoCenter(_geoCenter), activeTaxels(_activeTaxels), taxelList(vector<unsigned int>(activeTaxels, 0)), pressure(_pressure), normalDir(zeros(3)){}
32 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
33 skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
34  const yarp::sig::Vector &_geoCenter, unsigned int _activeTaxels, double _pressure, const Vector &_normalDir)
35 :dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
36 geoCenter(_geoCenter), activeTaxels(_activeTaxels), taxelList(vector<unsigned int>(activeTaxels, 0)), pressure(_pressure), normalDir(_normalDir){}
37 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
38 skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
39  const yarp::sig::Vector &_geoCenter, vector<unsigned int> _taxelList, double _pressure)
40 :dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
41 geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(zeros(3)){}
42 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
43 skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
44  const yarp::sig::Vector &_geoCenter, vector<unsigned int> _taxelList, double _pressure, const Vector &_normalDir)
45 :dynContact(_bodyPart, _linkNumber, _CoP), skinPart(_skinPart),
46 geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(_normalDir){}
47 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
48  skinContact::skinContact(const BodyPart &_bodyPart, const SkinPart &_skinPart, unsigned int _linkNumber, const yarp::sig::Vector &_CoP,
49  const yarp::sig::Vector &_geoCenter, std::vector<unsigned int> _taxelList, double _pressure, const yarp::sig::Vector &_normalDir,
50  const yarp::sig::Vector &_F, const yarp::sig::Vector &_Mu)
51  :dynContact(_bodyPart,_linkNumber,_CoP,_Mu), skinPart(_skinPart),
52  geoCenter(_geoCenter), taxelList(_taxelList), activeTaxels(_taxelList.size()), pressure(_pressure), normalDir(_normalDir){
53  this->setForce(_F); //note that dynContact constructor sets Fmodule to 0; here setForce() overwrites the init with the proper force vector and sets
54  //also Fmodule and Fdir appropriately
55  }
56 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
59 //~~~~~~~~~~~~~~~~~~~~~~
60 // SET methods
61 //~~~~~~~~~~~~~~~~~~~~~~
62 bool skinContact::setGeoCenter(const Vector &_geoCenter){
63  if(!checkVectorDim(_geoCenter, 3, "Geometric center"))
64  return false;
65  geoCenter = _geoCenter;
66  return true;
67 }
68 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
69 bool skinContact::setNormalDir(const Vector &_normalDir){
70  if(!checkVectorDim(_normalDir, 3, "Normal direction"))
71  return false;
72  normalDir = _normalDir;
73  return true;
74 }
75 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
76 bool skinContact::setPressure(double _pressure){
77  if(pressure<=0.0)
78  return false;
79  pressure = _pressure;
80  return true;
81 }
82 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
83 void skinContact::setActiveTaxels(unsigned int _activeTaxels){
84  activeTaxels = _activeTaxels;
85  taxelList.resize(activeTaxels);
86 }
87 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
89  skinPart = _skinPart;
90 }
91 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
92 void skinContact::setTaxelList(const vector<unsigned int> &list){
93  taxelList = list;
94  activeTaxels = list.size();
95 }
96 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
97 // SERIALIZATION methods
98 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
99 bool skinContact::write(ConnectionWriter& connection) const
100 {
101  // represent a skinContact as a list of 8 elements that are:
102  // - a list of 4 int, i.e. contactId, bodyPart, linkNumber, skinPart
103  // - a list of 3 double, i.e. the CoP
104  // - a list of 3 double, i.e. the force
105  // - a list of 3 double, i.e. the moment
106  // - a list of 3 double, i.e. the geometric center
107  // - a list of 3 double, i.e. the normal direction
108  // - a list of N int, i.e. the active taxel ids
109  // - a double, i.e. the pressure
110 
111  connection.appendInt(BOTTLE_TAG_LIST);
112  connection.appendInt(8);
113  // list of 4 int, i.e. contactId, bodyPart, linkNumber, skinPart
114  connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_INT);
115  connection.appendInt(4);
116  connection.appendInt(contactId);
117  connection.appendInt(bodyPart); // left_arm, right_arm, ...
118  connection.appendInt(linkNumber);
119  connection.appendInt(skinPart);
120  // list of 3 double, i.e. the CoP
121  connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
122  connection.appendInt(3);
123  for(int i=0;i<3;i++) connection.appendDouble(CoP[i]);
124  // list of 3 double, i.e. the force
125  connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
126  connection.appendInt(3);
127  for(int i=0;i<3;i++) connection.appendDouble(F[i]);
128  // list of 3 double, i.e. the moment
129  connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
130  connection.appendInt(3);
131  for(int i=0;i<3;i++) connection.appendDouble(Mu[i]);
132  // - a list of 3 double, i.e. the geometric center
133  connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
134  connection.appendInt(3);
135  for(int i=0;i<3;i++) connection.appendDouble(geoCenter[i]);
136  // - a list of 3 double, i.e. the normal direction
137  connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_DOUBLE);
138  connection.appendInt(3);
139  for(int i=0;i<3;i++) connection.appendDouble(normalDir[i]);
140  // - a list of N int, i.e. the active taxel ids
141  connection.appendInt(BOTTLE_TAG_LIST + BOTTLE_TAG_INT);
142  connection.appendInt(activeTaxels);
143  for(unsigned int i=0;i<activeTaxels;i++) connection.appendInt(taxelList[i]);
144  // - a double, i.e. the pressure
145  connection.appendInt(BOTTLE_TAG_DOUBLE);
146  connection.appendDouble(pressure);
147 
148  // if someone is foolish enough to connect in text mode,
149  // let them see something readable.
150  connection.convertTextMode();
151 
152  return !connection.isError();
153 }
154 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
155 bool skinContact::read(ConnectionReader& connection){
156  // auto-convert text mode interaction
157  connection.convertTextMode();
158 
159  // represent a skinContact as a list of 8 elements that are:
160  // - a list of 4 int, i.e. contactId, bodyPart, linkNumber, skinPart
161  // - a list of 3 double, i.e. the CoP
162  // - a list of 3 double, i.e. the force
163  // - a list of 3 double, i.e. the moment
164  // - a list of 3 double, i.e. the geometric center
165  // - a list of 3 double, i.e. the normal direction
166  // - a list of N int, i.e. the active taxel ids
167  // - a double, i.e. the pressure
168  if(connection.expectInt() != BOTTLE_TAG_LIST || connection.expectInt() != 8)
169  return false;
170 
171  // - a list of 4 int, i.e. contactId, bodyPart, linkNumber, skinPart
172  if(connection.expectInt()!=BOTTLE_TAG_LIST+BOTTLE_TAG_INT || connection.expectInt()!=4)
173  return false;
174  contactId = connection.expectInt();
175  bodyPart = (BodyPart)connection.expectInt();
176  linkNumber = connection.expectInt();
177  skinPart = (SkinPart) connection.expectInt();
178 
179  // - a list of 3 double, i.e. the CoP
180  if(connection.expectInt()!=BOTTLE_TAG_LIST+BOTTLE_TAG_DOUBLE || connection.expectInt()!=3)
181  return false;
182  for(int i=0;i<3;i++) CoP[i] = connection.expectDouble();
183 
184  // - a list of 3 double, i.e. the force
185  if(connection.expectInt()!=BOTTLE_TAG_LIST+BOTTLE_TAG_DOUBLE || connection.expectInt()!=3)
186  return false;
187  for(int i=0;i<3;i++) F[i] = connection.expectDouble();
188  setForce(F);
189 
190  // - a list of 3 double, i.e. the moment
191  if(connection.expectInt()!=BOTTLE_TAG_LIST+BOTTLE_TAG_DOUBLE || connection.expectInt()!=3)
192  return false;
193  for(int i=0;i<3;i++) Mu[i] = connection.expectDouble();
194 
195  // - a list of 3 double, i.e. the geometric center
196  if(connection.expectInt()!=BOTTLE_TAG_LIST+BOTTLE_TAG_DOUBLE || connection.expectInt()!=3)
197  return false;
198  for(int i=0;i<3;i++) geoCenter[i] = connection.expectDouble();
199 
200  // - a list of 3 double, i.e. the normal direction
201  if(connection.expectInt()!=BOTTLE_TAG_LIST+BOTTLE_TAG_DOUBLE || connection.expectInt()!=3)
202  return false;
203  for(int i=0;i<3;i++) normalDir[i] = connection.expectDouble();
204 
205  // - a list of N int, i.e. the active taxel ids
206  if(connection.expectInt()!=BOTTLE_TAG_LIST+BOTTLE_TAG_INT)
207  return false;
208  activeTaxels = connection.expectInt();
209  taxelList.resize(activeTaxels);
210  for(unsigned int i=0;i<activeTaxels;i++) taxelList[i] = connection.expectInt();
211 
212  // - a double, i.e. the pressure
213  if(connection.expectInt()!=BOTTLE_TAG_DOUBLE)
214  return false;
215  pressure = connection.expectDouble();
216 
217  return !connection.isError();
218 }
219 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
220 Vector skinContact::toVector() const{
221  Vector v(activeTaxels+21);
222  unsigned int index = 0;
223  v[index++] = contactId;
224  v[index++] = bodyPart;
225  v[index++] = linkNumber;
226  v[index++] = skinPart;
227  v.setSubvector(index, CoP); index+=3;
228  v.setSubvector(index, F); index+=3;
229  v.setSubvector(index, Mu); index+=3;
230  v.setSubvector(index, geoCenter); index+=3;
231  v.setSubvector(index, normalDir); index+=3;
232  v[index++] = activeTaxels;
233  for(unsigned int i=0;i<activeTaxels;i++)
234  v[index++] = taxelList[i];
235  v[index++] = pressure;
236 
237  return v;
238 }
239 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
240 bool skinContact::fromVector(const Vector &v){
241  if(v.size()<21)
242  return false;
243  unsigned int index = 0;
244  contactId = (unsigned long)(v[index++]);
245  bodyPart = (BodyPart) int(v[index++]);
246  linkNumber = (unsigned int)(v[index++]);
247  skinPart = (SkinPart) int(v[index++]);
248  CoP = v.subVector(index, index+2); index+=3;
249  F = v.subVector(index, index+2); index+=3;
250  Mu = v.subVector(index, index+2); index+=3;
251  geoCenter = v.subVector(index, index+2); index+=3;
252  normalDir = v.subVector(index, index+2); index+=3;
253  activeTaxels = (unsigned int)(v[index++]);
254  if(v.size()!=21+activeTaxels)
255  return false;
256  taxelList.resize(activeTaxels);
257  for(unsigned int i=0;i<activeTaxels;i++)
258  taxelList[i] = (unsigned int)(v[index++]);
259  pressure = v[index++];
260 
261  return true;
262 }
263 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
264 string skinContact::toString(int precision) const{
265  stringstream res;
266  res<< dynContact::toString(precision)<< ", Skin part: "<< SkinPart_s[skinPart]<< ", geometric center: "<<
267  geoCenter.toString(precision)<< ", normal direction: "<< normalDir.toString(precision)<<
268  ", active taxels: "<< activeTaxels<< ", pressure: "<< pressure;
269  return res.str();
270 }
271 //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
272 
273 
const std::string SkinPart_s[]
Definition: common.h:64
virtual bool setForce(const yarp::sig::Vector &_F)
Set the contact force.
Definition: dynContact.cpp:101
yarp::sig::Vector CoP
center of pressure of the contact expressed w.r.t. the reference frame of the link ...
Definition: dynContact.h:64
bool setNormalDir(const yarp::sig::Vector &_normalDir)
Set the normal direction of the contact area, expressed in link reference frame.
Definition: skinContact.cpp:69
zeros(2, 2) eye(2
Class representing an external contact acting on a link of the robot body.
Definition: dynContact.h:51
void setActiveTaxels(unsigned int _activeTaxels)
Set the number of active taxels.
Definition: skinContact.cpp:83
STL namespace.
yarp::sig::Vector Mu
contact moment
Definition: dynContact.h:72
yarp::sig::Vector F
contact force
Definition: dynContact.h:68
virtual std::string toString(int precision=-1) const
Convert this contact into a string.
Definition: dynContact.cpp:263
void setSkinPart(SkinPart _skinPart)
Set the skin part on which this contact is applied.
Definition: skinContact.cpp:88
unsigned int linkNumber
number of the link where the contact is applied
Definition: dynContact.h:62
yarp::sig::Vector normalDir
Definition: skinContact.h:60
bool checkVectorDim(const yarp::sig::Vector &v, unsigned int dim, const std::string &descr="")
Definition: dynContact.cpp:274
virtual std::string toString(int precision=-1) const override
Convert this skinContact into a string.
virtual bool fromVector(const yarp::sig::Vector &v)
Convert the specified vector into a skinContact.
static int v
Definition: iCub_Sim.cpp:42
virtual bool read(yarp::os::ConnectionReader &connection) override
bool setPressure(double _pressure)
Set the average contact pressure.
Definition: skinContact.cpp:76
BodyPart bodyPart
part of the body of the robot where the contact is applied
Definition: dynContact.h:60
virtual yarp::sig::Vector toVector() const
Convert this skinContact to a vector.
bool setGeoCenter(const yarp::sig::Vector &_geoCenter)
Set the geometric center of the contact area (link reference frame).
Definition: skinContact.cpp:62
virtual bool write(yarp::os::ConnectionWriter &connection) const override
Write this skinContact to a connection.
Definition: skinContact.cpp:99
skinContact()
Empty contructor.
Definition: skinContact.cpp:57
dynContact()
Default constructor.
Definition: dynContact.cpp:33
Class that encloses everything relate to a skinPart.
Definition: skinPart.h:145
void setTaxelList(const std::vector< unsigned int > &list)
Set the list of taxels that are activated by this contact.
Definition: skinContact.cpp:92
std::vector< unsigned int > taxelList
Definition: skinContact.h:64
yarp::sig::Vector geoCenter
Definition: skinContact.h:58