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 
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[LEFT_LEG_UPPER])
60  return LEFT_LEG_UPPER;
61  else if(skinPartString == SkinPart_s[LEFT_LEG_LOWER])
62  return LEFT_LEG_LOWER;
63  else if(skinPartString == SkinPart_s[LEFT_FOOT])
64  return LEFT_FOOT;
65  else if(skinPartString == SkinPart_s[RIGHT_LEG_UPPER])
66  return RIGHT_LEG_UPPER;
67  else if(skinPartString == SkinPart_s[RIGHT_LEG_LOWER])
68  return RIGHT_LEG_LOWER;
69  else if(skinPartString == SkinPart_s[RIGHT_FOOT])
70  return 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 }
iCub::skinDynLib::SKIN_FRONT_TORSO
@ SKIN_FRONT_TORSO
Definition: common.h:60
iCub::skinDynLib::BODY_PART_UNKNOWN
@ BODY_PART_UNKNOWN
Definition: common.h:45
iCub::skinDynLib::SKIN_LEFT_HAND
@ SKIN_LEFT_HAND
Definition: common.h:58
iCub::skinDynLib::SkinPart_2_BodyPart
const Skin_2_Body SkinPart_2_BodyPart[SKIN_PART_SIZE]
Definition: common.h:77
iCub::skinDynLib
Definition: common.h:35
iCub::skinDynLib::LEFT_LEG_LOWER
@ LEFT_LEG_LOWER
Definition: common.h:60
iCub::skinDynLib::getSkinPartFromString
SkinPart getSkinPartFromString(const std::string skinPartString)
Definition: common.cpp:42
iCub::skinDynLib::SkinPart_2_LinkNum
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE]
Definition: common.h:91
iCub::skinDynLib::RIGHT_LEG_LOWER
@ RIGHT_LEG_LOWER
Definition: common.h:61
iCub::skinDynLib::SKIN_RIGHT_UPPER_ARM
@ SKIN_RIGHT_UPPER_ARM
Definition: common.h:59
iCub::skinDynLib::matrixFromBottle
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
iCub::skinDynLib::getLinkNum
int getLinkNum(SkinPart s)
Definition: common.cpp:34
iCub::skinDynLib::Skin_2_Body::body
BodyPart body
Definition: common.h:76
iCub::skinDynLib::toVector
yarp::sig::Vector toVector(yarp::sig::Matrix m)
Definition: common.cpp:77
iCub::skinDynLib::SKIN_PART_UNKNOWN
@ SKIN_PART_UNKNOWN
Definition: common.h:57
iCub::skinDynLib::LEFT_LEG_UPPER
@ LEFT_LEG_UPPER
Definition: common.h:60
string
string(REPLACE "-rdynamic" "" CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS "${CMAKE_SHARED_LIBRARY_LINK_CXX_FLAGS}") include_directories($
Definition: CMakeLists.txt:9
iCub::skinDynLib::SKIN_PART_SIZE
@ SKIN_PART_SIZE
Definition: common.h:62
iCub::skinDynLib::SkinPart
SkinPart
Definition: common.h:56
iCub::skinDynLib::RIGHT_FOOT
@ RIGHT_FOOT
Definition: common.h:61
iCub::skinDynLib::SkinPart_s
const std::string SkinPart_s[]
Definition: common.h:64
iCub::skinDynLib::LEFT_FOOT
@ LEFT_FOOT
Definition: common.h:60
iCub::skinDynLib::RIGHT_LEG_UPPER
@ RIGHT_LEG_UPPER
Definition: common.h:61
common.h
iCub::skinDynLib::BodyPart
BodyPart
Definition: common.h:44
iCub::skinDynLib::SKIN_RIGHT_FOREARM
@ SKIN_RIGHT_FOREARM
Definition: common.h:59
v
static int v
Definition: iCub_Sim.cpp:42
iCub::skinDynLib::SKIN_RIGHT_HAND
@ SKIN_RIGHT_HAND
Definition: common.h:59
iCub::skinDynLib::getBodyPart
BodyPart getBodyPart(SkinPart s)
Definition: common.cpp:26
iCub::skinDynLib::SKIN_LEFT_FOREARM
@ SKIN_LEFT_FOREARM
Definition: common.h:58
iCub::skinDynLib::matrixIntoBottle
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
iCub::skinDynLib::vectorFromBottle
yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size)
Definition: common.cpp:89
iCub::skinDynLib::SKIN_LEFT_UPPER_ARM
@ SKIN_LEFT_UPPER_ARM
Definition: common.h:58
iCub::skinDynLib::getSkinParts
std::vector< SkinPart > getSkinParts(BodyPart b)
Definition: common.cpp:17
iCub::skinDynLib::vectorIntoBottle
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