iCub-main
Taxel.cpp
Go to the documentation of this file.
2 
3 using namespace yarp::math;
4 using namespace iCub::skinDynLib;
5 
6 /****************************************************************/
7 /* TAXEL WRAPPER
8 *****************************************************************/
9  Taxel::Taxel()
10  {
11  init();
12  }
13 
14  Taxel::Taxel(const yarp::sig::Vector &_position, const yarp::sig::Vector &_normal)
15  {
16  init();
17  Position = _position;
18  Normal = _normal;
19  setFoR();
20  }
21 
22  Taxel::Taxel(const yarp::sig::Vector &_position, const yarp::sig::Vector &_normal, const int &_id)
23  {
24  init();
25  ID = _id;
26  Position = _position;
27  Normal = _normal;
28  setFoR();
29  }
30 
31  Taxel::Taxel(const Taxel &_t)
32  {
33  *this = _t;
34  }
35 
36  Taxel & Taxel::operator=(const Taxel &_t)
37  {
38  if (this == &_t)
39  {
40  return *this;
41  }
42 
43  ID = _t.ID;
44  Position = _t.Position;
45  WRFPosition = _t.WRFPosition;
46  Normal = _t.Normal;
47  px = _t.px;
48  FoR = _t.FoR;
49  return *this;
50  }
51 
52  void Taxel::init()
53  {
54  ID = 0;
55  Position.resize(3,0.0);
56  WRFPosition.resize(3,0.0);
57  Normal.resize(3,0.0);
58  px.resize(2,0.0);
59  FoR = eye(4);
60  }
61 
62  void Taxel::setFoR()
63  {
64  if (Normal == zeros(3))
65  {
66  FoR=eye(4);
67  return;
68  }
69 
70  // Set the proper orientation for the touching end-effector
71  yarp::sig::Vector x(3,0.0), z(3,0.0), y(3,0.0);
72 
73  z = Normal;
74  if (z[0] == 0.0)
75  {
76  z[0] = 0.00000001; // Avoid the division by 0
77  }
78  y[0] = -z[2]/z[0]; y[2] = 1;
79  x = -1*(cross(z,y));
80 
81  // Let's make them unitary vectors:
82  x = x / norm(x);
83  y = y / norm(y);
84  z = z / norm(z);
85 
86  FoR=eye(4);
87  FoR.setSubcol(x,0,0);
88  FoR.setSubcol(y,0,1);
89  FoR.setSubcol(z,0,2);
90  FoR.setSubcol(Position,0,3);
91  }
92 
93  int Taxel::getID()
94  {
95  return ID;
96  }
97 
98  yarp::sig::Vector Taxel::getPosition()
99  {
100  return Position;
101  }
102 
103  yarp::sig::Vector Taxel::getNormal()
104  {
105  return Normal;
106  }
107 
108  yarp::sig::Vector Taxel::getWRFPosition()
109  {
110  return WRFPosition;
111  }
112 
113  yarp::sig::Vector Taxel::getPx()
114  {
115  return px;
116  }
117 
118  yarp::sig::Matrix Taxel::getFoR()
119  {
120  return FoR;
121  }
122 
123  bool Taxel::setID(int _ID)
124  {
125  ID = _ID;
126  return true;
127  }
128 
129  bool Taxel::setPosition(const yarp::sig::Vector &_Position)
130  {
131  if (_Position.size()!=3)
132  {
133  return false;
134  }
135 
136  Position=_Position;
137  return true;
138  }
139 
140  bool Taxel::setNormal(const yarp::sig::Vector &_Normal)
141  {
142  if (_Normal.size()!=3)
143  {
144  return false;
145  }
146 
147  Normal=_Normal;
148  return true;
149  }
150 
151  bool Taxel::setWRFPosition(const yarp::sig::Vector &_WRFPosition)
152  {
153  if (_WRFPosition.size()!=3)
154  {
155  return false;
156  }
157 
158  WRFPosition=_WRFPosition;
159  return true;
160  }
161 
162  bool Taxel::setPx(const yarp::sig::Vector &_px)
163  {
164  if (_px.size()!=2)
165  {
166  return false;
167  }
168 
169  px=_px;
170  return true;
171  }
172 
174  {
175  if (verbosity)
176  {
177  yDebug("ID %i \tPosition %s \tNormal %s \tWRFPosition %s \tpx %s", ID,
178  Position.toString(3,3).c_str(), Normal.toString(3,3).c_str(),
179  WRFPosition.toString(3,3).c_str(),px.toString(3,3).c_str());
180  yDebug("\tFrame of Reference \n%s",FoR.toString(3,3).c_str());
181  }
182  else
183  yDebug("ID %i \tPosition %s \tNormal %s\n", ID,
184  Position.toString(3,3).c_str(), Normal.toString(3,3).c_str());
185  }
186 
188  {
189  std::stringstream res;
190  res << "ID: " << ID << "\tPosition: "<< Position.toString(3,3) <<
191  "\tNormal: "<< Normal.toString(3,3);
192 
193  if (verbosity)
194  {
195  res << "\tWRFPosition: " << WRFPosition.toString(3,3) <<
196  "\tPx: " << px.toString(3,3) <<
197  "\tFrame of Reference: \n" << FoR.toString(3,3) << std::endl;
198  }
199  return res.str();
200  }
201 
202 // empty line to make gcc happy
iCub::skinDynLib
Definition: common.h:35
iCub::skinDynLib::Taxel
Definition: Taxel.h:56
z
z
Definition: show_eyes_axes.m:22
iCub::skinDynLib::Taxel::WRFPosition
yarp::sig::Vector WRFPosition
Definition: Taxel.h:62
Taxel.h
iCub::skinDynLib::Taxel::FoR
yarp::sig::Matrix FoR
Definition: Taxel.h:64
verbosity
int verbosity
Definition: main.cpp:21
iCub::skinManager::toString
std::string toString(const T &t)
Definition: compensator.h:199
iCub::ctrl::norm
double norm(const yarp::sig::Matrix &M, int col)
eth::parser::print
bool print(const pc104Data &pc104data)
Definition: ethParser.cpp:57
y
y
Definition: show_eyes_axes.m:21
x
x
Definition: compute_ekf_sym.m:21
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::ctrl::cross
yarp::sig::Vector cross(const yarp::sig::Matrix &A, int colA, const yarp::sig::Matrix &B, int colB)
iCub::skinDynLib::Taxel::Position
yarp::sig::Vector Position
Definition: Taxel.h:60
iCub::skinDynLib::Taxel::px
yarp::sig::Vector px
Definition: Taxel.h:63
iCub::skinDynLib::Taxel::Normal
yarp::sig::Vector Normal
Definition: Taxel.h:61
iCub::skinDynLib::Taxel::ID
int ID
Definition: Taxel.h:59
zeros
zeros(2, 2) eye(2