iCub-main
common.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 <iomanip>
10 #include <string>
11 #include "iCub/skinDynLib/common.h"
12 
13 using namespace std;
14 using namespace yarp::sig;
15 using namespace iCub::skinDynLib;
16 
18 {
19  vector<SkinPart> res;
20  for(unsigned int i=0; i<SKIN_PART_SIZE; i++)
21  if(SkinPart_2_BodyPart[i].body==b)
22  res.push_back(SkinPart_2_BodyPart[i].skin);
23  return res;
24 }
25 
27 {
28  for(unsigned int i=0; i<SKIN_PART_SIZE; i++)
29  if(SkinPart_2_BodyPart[i].skin==s)
30  return SkinPart_2_BodyPart[i].body;
31  return BODY_PART_UNKNOWN;
32 }
33 
35 {
36  for(unsigned int i=0; i<SKIN_PART_SIZE; i++)
37  if(SkinPart_2_LinkNum[i].skin==s)
38  return SkinPart_2_LinkNum[i].linkNum;
39  return -1;
40 }
41 
42 SkinPart iCub::skinDynLib::getSkinPartFromString(const std::string skinPartString)
43 {
44 
45  if (skinPartString == SkinPart_s[SKIN_LEFT_HAND])
46  return SKIN_LEFT_HAND;
47  else if(skinPartString == SkinPart_s[SKIN_LEFT_FOREARM])
48  return SKIN_LEFT_FOREARM;
49  else if(skinPartString == SkinPart_s[SKIN_LEFT_UPPER_ARM])
50  return SKIN_LEFT_UPPER_ARM;
51  else if(skinPartString == SkinPart_s[SKIN_RIGHT_HAND])
52  return SKIN_RIGHT_HAND;
53  else if(skinPartString == SkinPart_s[SKIN_RIGHT_FOREARM])
54  return SKIN_RIGHT_FOREARM;
55  else if(skinPartString == SkinPart_s[SKIN_RIGHT_UPPER_ARM])
56  return SKIN_RIGHT_UPPER_ARM;
57  else if(skinPartString == SkinPart_s[SKIN_FRONT_TORSO])
58  return SKIN_FRONT_TORSO;
59  else if(skinPartString == SkinPart_s[SKIN_LEFT_LEG_UPPER])
60  return SKIN_LEFT_LEG_UPPER;
61  else if(skinPartString == SkinPart_s[SKIN_LEFT_LEG_LOWER])
62  return SKIN_LEFT_LEG_LOWER;
63  else if(skinPartString == SkinPart_s[SKIN_LEFT_FOOT])
64  return SKIN_LEFT_FOOT;
65  else if(skinPartString == SkinPart_s[SKIN_RIGHT_LEG_UPPER])
66  return SKIN_RIGHT_LEG_UPPER;
67  else if(skinPartString == SkinPart_s[SKIN_RIGHT_LEG_LOWER])
68  return SKIN_RIGHT_LEG_LOWER;
69  else if(skinPartString == SkinPart_s[SKIN_RIGHT_FOOT])
70  return SKIN_RIGHT_FOOT;
71  else
72  return SKIN_PART_UNKNOWN;
73 
74 
75 }
76 
77 yarp::sig::Vector iCub::skinDynLib::toVector(yarp::sig::Matrix m)
78 {
79  Vector res(m.rows()*m.cols(),0.0);
80 
81  for (int r = 0; r < m.rows(); r++)
82  {
83  res.setSubvector(r*m.cols(),m.getRow(r));
84  }
85 
86  return res;
87 }
88 
89 yarp::sig::Vector iCub::skinDynLib::vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
90 {
91  yarp::sig::Vector v(size,0.0);
92 
93  for (int i = 0; i < size; i++)
94  {
95  v[i] = b.get(in).asFloat64();
96  in++;
97  }
98  return v;
99 }
100 
101 void iCub::skinDynLib::vectorIntoBottle(const yarp::sig::Vector v, yarp::os::Bottle &b)
102 {
103  for (unsigned int i = 0; i < v.size(); i++)
104  {
105  b.addFloat64(v[i]);
106  }
107 }
108 
109 yarp::sig::Matrix iCub::skinDynLib::matrixFromBottle(const yarp::os::Bottle b, int in, const int r, const int c)
110 {
111  yarp::sig::Matrix m(r,c);
112  m.zero();
113 
114  for (int i = 0; i<r; i++)
115  {
116  for (int j = 0; j<c; j++)
117  {
118  m(i,j) = b.get(in).asFloat64();
119  in++;
120  }
121  }
122 
123  return m;
124 }
125 
126 void iCub::skinDynLib::matrixIntoBottle(const yarp::sig::Matrix m, yarp::os::Bottle &b)
127 {
128  Vector v = toVector(m);
129 
130  for (unsigned int i = 0; i < v.size(); i++)
131  {
132  b.addFloat64(v[i]);
133  }
134 }
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
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
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
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
@ 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
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