iCub-main
common.h
Go to the documentation of this file.
1 
22 #ifndef __COMMON_H__
23 #define __COMMON_H__
24 
25 #include <yarp/sig/Vector.h>
26 #include <yarp/sig/Matrix.h>
27 #include <string>
28 #include <vector>
29 #include <map>
30 #include <list>
31 
32 namespace iCub
33 {
34 
35 namespace skinDynLib
36 {
37 
39 
40 // DEFINE BODY PARTS AND SKIN PARTS OF ICUB ROBOT
41 #ifndef NAO_SKIN
42 
43 // List of the parts of the iCub body
44 enum BodyPart {
48 };
49 const std::string BodyPart_s[] = {
50  "unknown_body_part",
51  "torso", "head", "left_arm", "right_arm", "left_leg", "right_leg",
52  "all_body_parts", "lower_body_parts", "upper_body_parts", "body_part_size"
53 };
54 
55 // List of the parts composing the skin of iCub
56 enum SkinPart {
63 };
64 const std::string SkinPart_s[] = {
65  "unknown",
66  "l_hand", "l_forearm", "l_upper_arm",
67  "r_hand", "r_forearm", "r_upper_arm",
68  "chest",
69  "l_upper_leg", "l_lower_leg", "l_foot",
70  "r_upper_leg", "r_lower_leg", "r_foot",
71  "all_skin_parts", "skin_part_size"
72 };
73 
74 // *** Mapping from SKIN PARTs to BODY PARTs ***
75 // To represent this information we use a vector of struct{SkinPart; BodyPart;}
93 };
94 
95 // association between each skin part and the number of the link where it is mounted
96 struct Skin_2_Link{ SkinPart skin; int linkNum; };
98  {SKIN_PART_UNKNOWN, -1},
99  {SKIN_LEFT_HAND, 6},
100  {SKIN_LEFT_FOREARM, 4},
101  {SKIN_LEFT_UPPER_ARM, 2},
102  {SKIN_RIGHT_HAND, 6},
103  {SKIN_RIGHT_FOREARM, 4},
105  {SKIN_FRONT_TORSO, 2},
106  {SKIN_PART_ALL, -1}
107 };
108 #endif
109 
110 // DEFINE BODY PARTS AND SKIN PARTS OF NAO ROBOT
111 #ifdef NAO_SKIN
112 // List of the parts of the iCub body
113 enum BodyPart {
117 };
118 const std::string BodyPart_s[] = {
119  "unknown_body_part",
120  "head", "torso", "left_arm", "right_arm", "left_leg", "right_leg",
121  "all_body_parts", "body_part_size"
122 };
123 
124 // List of the parts composing the skin of iCub
125 enum SkinPart {
127  SKIN_LEFT_HAND, SKIN_LEFT_ARM,
128  SKIN_RIGHT_HAND, SKIN_RIGHT_ARM,
130 };
131 const std::string SkinPart_s[] = {
132  "unknown_skin_part",
133  "skin_left_hand", "skin_left_arm",
134  "skin_right_hand", "skin_right_arm",
135  "all_skin_parts", "skin_part_size"
136 };
137 
138 // association between each skin part and the corresponding body part
139 struct Skin_2_Body { SkinPart skin; BodyPart body; };
140 const Skin_2_Body SkinPart_2_BodyPart[SKIN_PART_SIZE] = {
143  {SKIN_LEFT_ARM, LEFT_ARM},
145  {SKIN_RIGHT_ARM, RIGHT_ARM},
147 };
148 
149 // association between each skin part and the number of the link where it is mounted
150 struct Skin_2_Link{ SkinPart skin; int linkNum; };
151 const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE] = {
152  {SKIN_PART_UNKNOWN, -1},
153  {SKIN_LEFT_HAND, -1},
154  {SKIN_LEFT_ARM, -1},
155  {SKIN_RIGHT_HAND, -1},
156  {SKIN_RIGHT_ARM, -1},
157  {SKIN_PART_ALL, -1}
158 };
159 #endif
160 
161 enum HandPart {
164 };
165 const std::string HandPart_s[] = {
166  "index", "middle", "ring", "little", "thumb", "palm",
167  "all_hand_parts", "hand_part_size"
168 };
169 
170 static const int ARM_FT_SENSOR_LINK_INDEX = 2; // index of the link containing the F/T sensor in the iCub arms
171 static const int LEG_FT_SENSOR_LINK_INDEX = 1; // index of the link containing the F/T sensor in the iCub legs
172 
173 static const int ARM_DOF = 7;
174 static const int TORSO_DOF = 3;
175 
184 
192 std::vector<SkinPart> getSkinParts(BodyPart b);
193 
201 int getLinkNum(SkinPart s);
202 
210 SkinPart getSkinPartFromString(const std::string skinPartString);
211 
212 
213 
220 yarp::sig::Vector toVector(yarp::sig::Matrix m);
221 
230 yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size);
231 
238 void vectorIntoBottle(const yarp::sig::Vector v, yarp::os::Bottle &b);
239 
248 yarp::sig::Matrix matrixFromBottle(const yarp::os::Bottle b, int in, const int r, const int c);
249 
256 void matrixIntoBottle(const yarp::sig::Matrix m, yarp::os::Bottle &b);
257 
265 inline std::list<unsigned int> vectorofIntEqualto(const std::vector<int> vec, const int val)
266 {
267  std::list<unsigned int> res;
268  for (size_t i = 0; i < vec.size(); i++)
269  {
270  if (vec[i]==val)
271  {
272  res.push_back(i);
273  }
274  }
275  return res;
276 };
277 
278 }
279 
280 }//end namespace
281 
282 #endif
283 
284 
SkinPart getSkinPartFromString(const std::string skinPartString)
Get the SkinPart enum from the string version - essentially the opposite of SkinPart_s[].
Definition: common.cpp:42
int getLinkNum(SkinPart s)
Get the link number associated to the specified skin part.
Definition: common.cpp:34
std::vector< SkinPart > getSkinParts(BodyPart b)
Get the list of skin parts associated to the specified body part.
Definition: common.cpp:17
std::list< unsigned int > vectorofIntEqualto(const std::vector< int > vec, const int val)
Returns a list of indexes corresponding to the values of vec that are equal to val.
Definition: common.h:265
yarp::sig::Vector toVector(yarp::sig::Matrix m)
Converts a yarp::sig::Matrix to a yarp::sig::Vector.
Definition: common.cpp:77
yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
Retrieves a vector from a bottle.
Definition: common.cpp:89
BodyPart getBodyPart(SkinPart s)
Get the body part associated to the specified skin part.
Definition: common.cpp:26
const std::string HandPart_s[]
Definition: common.h:165
void matrixIntoBottle(const yarp::sig::Matrix m, yarp::os::Bottle &b)
Puts a matrix into a bottle, by cycling through its elements and adding them as double.
Definition: common.cpp:126
const Skin_2_Body SkinPart_2_BodyPart[SKIN_PART_SIZE]
Definition: common.h:77
const std::string SkinPart_s[]
Definition: common.h:64
const std::string BodyPart_s[]
Definition: common.h:49
static const int ARM_DOF
Definition: common.h:173
void vectorIntoBottle(const yarp::sig::Vector v, yarp::os::Bottle &b)
Puts a Vector into a bottle, by cycling through its elements and adding them as doubles.
Definition: common.cpp:101
static const int LEG_FT_SENSOR_LINK_INDEX
Definition: common.h:171
@ SKIN_LEFT_LEG_UPPER
Definition: common.h:60
@ SKIN_FRONT_TORSO
Definition: common.h:60
@ SKIN_PART_UNKNOWN
Definition: common.h:57
@ SKIN_LEFT_UPPER_ARM
Definition: common.h:58
@ SKIN_RIGHT_UPPER_ARM
Definition: common.h:59
@ SKIN_LEFT_LEG_LOWER
Definition: common.h:60
@ SKIN_RIGHT_LEG_LOWER
Definition: common.h:61
@ SKIN_RIGHT_LEG_UPPER
Definition: common.h:61
@ SKIN_LEFT_FOREARM
Definition: common.h:58
@ SKIN_RIGHT_FOREARM
Definition: common.h:59
@ BODY_PART_UNKNOWN
Definition: common.h:45
@ UPPER_BODY_PARTS
Definition: common.h:47
@ LOWER_BODY_PARTS
Definition: common.h:47
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE]
Definition: common.h:97
yarp::sig::Matrix matrixFromBottle(const yarp::os::Bottle b, int in, const int r, const int c)
Retrieves a matrix from a bottle.
Definition: common.cpp:109
static const int ARM_FT_SENSOR_LINK_INDEX
Definition: common.h:170
static const int TORSO_DOF
Definition: common.h:174
This file contains the definition of unique IDs for the body parts and the skin parts of the robot.