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;}
80  {SKIN_LEFT_FOREARM, LEFT_ARM},
81  {SKIN_LEFT_UPPER_ARM, LEFT_ARM},
83  {SKIN_RIGHT_FOREARM, RIGHT_ARM},
84  {SKIN_RIGHT_UPPER_ARM, RIGHT_ARM},
87 };
88 
89 // association between each skin part and the number of the link where it is mounted
90 struct Skin_2_Link{ SkinPart skin; int linkNum; };
92  {SKIN_PART_UNKNOWN, -1},
93  {SKIN_LEFT_HAND, 6},
94  {SKIN_LEFT_FOREARM, 4},
95  {SKIN_LEFT_UPPER_ARM, 2},
96  {SKIN_RIGHT_HAND, 6},
97  {SKIN_RIGHT_FOREARM, 4},
99  {SKIN_FRONT_TORSO, 2},
100  {SKIN_PART_ALL, -1}
101 };
102 #endif
103 
104 // DEFINE BODY PARTS AND SKIN PARTS OF NAO ROBOT
105 #ifdef NAO_SKIN
106 // List of the parts of the iCub body
107 enum BodyPart {
110  BODY_PART_ALL, BODY_PART_SIZE
111 };
112 const std::string BodyPart_s[] = {
113  "unknown_body_part",
114  "head", "torso", "left_arm", "right_arm", "left_leg", "right_leg",
115  "all_body_parts", "body_part_size"
116 };
117 
118 // List of the parts composing the skin of iCub
119 enum SkinPart {
121  SKIN_LEFT_HAND, SKIN_LEFT_ARM,
122  SKIN_RIGHT_HAND, SKIN_RIGHT_ARM,
123  SKIN_PART_ALL, SKIN_PART_SIZE
124 };
125 const std::string SkinPart_s[] = {
126  "unknown_skin_part",
127  "skin_left_hand", "skin_left_arm",
128  "skin_right_hand", "skin_right_arm",
129  "all_skin_parts", "skin_part_size"
130 };
131 
132 // association between each skin part and the corresponding body part
133 struct Skin_2_Body { SkinPart skin; BodyPart body; };
134 const Skin_2_Body SkinPart_2_BodyPart[SKIN_PART_SIZE] = {
137  {SKIN_LEFT_ARM, LEFT_ARM},
139  {SKIN_RIGHT_ARM, RIGHT_ARM},
140  {SKIN_PART_ALL, BODY_PART_ALL}
141 };
142 
143 // association between each skin part and the number of the link where it is mounted
144 struct Skin_2_Link{ SkinPart skin; int linkNum; };
145 const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE] = {
146  {SKIN_PART_UNKNOWN, -1},
147  {SKIN_LEFT_HAND, -1},
148  {SKIN_LEFT_ARM, -1},
149  {SKIN_RIGHT_HAND, -1},
150  {SKIN_RIGHT_ARM, -1},
151  {SKIN_PART_ALL, -1}
152 };
153 #endif
154 
155 enum HandPart {
158 };
159 const std::string HandPart_s[] = {
160  "index", "middle", "ring", "little", "thumb", "palm",
161  "all_hand_parts", "hand_part_size"
162 };
163 
164 static const int ARM_FT_SENSOR_LINK_INDEX = 2; // index of the link containing the F/T sensor in the iCub arms
165 static const int LEG_FT_SENSOR_LINK_INDEX = 1; // index of the link containing the F/T sensor in the iCub legs
166 
167 static const int ARM_DOF = 7;
168 static const int TORSO_DOF = 3;
169 
178 
186 std::vector<SkinPart> getSkinParts(BodyPart b);
187 
195 int getLinkNum(SkinPart s);
196 
204 SkinPart getSkinPartFromString(const std::string skinPartString);
205 
206 
207 
214 yarp::sig::Vector toVector(yarp::sig::Matrix m);
215 
224 yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size);
225 
232 void vectorIntoBottle(const yarp::sig::Vector v, yarp::os::Bottle &b);
233 
242 yarp::sig::Matrix matrixFromBottle(const yarp::os::Bottle b, int in, const int r, const int c);
243 
250 void matrixIntoBottle(const yarp::sig::Matrix m, yarp::os::Bottle &b);
251 
259 inline std::list<unsigned int> vectorofIntEqualto(const std::vector<int> vec, const int val)
260 {
261  std::list<unsigned int> res;
262  for (size_t i = 0; i < vec.size(); i++)
263  {
264  if (vec[i]==val)
265  {
266  res.push_back(i);
267  }
268  }
269  return res;
270 };
271 
272 }
273 
274 }//end namespace
275 
276 #endif
277 
278 
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE]
Definition: common.h:91
const std::string SkinPart_s[]
Definition: common.h:64
static const int LEG_FT_SENSOR_LINK_INDEX
Definition: common.h:165
yarp::sig::Vector toVector(yarp::sig::Matrix m)
Converts a yarp::sig::Matrix to a yarp::sig::Vector.
Definition: common.cpp:77
const std::string BodyPart_s[]
Definition: common.h:49
SkinPart getSkinPartFromString(const std::string skinPartString)
Get the SkinPart enum from the string version - essentially the opposite of SkinPart_s[].
Definition: common.cpp:42
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
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
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:259
static int v
Definition: iCub_Sim.cpp:42
static const int TORSO_DOF
Definition: common.h:168
static const int ARM_FT_SENSOR_LINK_INDEX
Definition: common.h:164
yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
Retrieves a vector from a bottle.
Definition: common.cpp:89
std::vector< SkinPart > getSkinParts(BodyPart b)
Get the list of skin parts associated to the specified body part.
Definition: common.cpp:17
const std::string HandPart_s[]
Definition: common.h:159
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
This file contains the definition of unique IDs for the body parts and the skin parts of the robot...
static const int ARM_DOF
Definition: common.h:167
const Skin_2_Body SkinPart_2_BodyPart[SKIN_PART_SIZE]
Definition: common.h:77
int getLinkNum(SkinPart s)
Get the link number associated to the specified skin part.
Definition: common.cpp:34
BodyPart getBodyPart(SkinPart s)
Get the body part associated to the specified skin part.
Definition: common.cpp:26