common.h

Go to the documentation of this file.

 
#ifndef __COMMON_H__
#define __COMMON_H__
 
#include <yarp/sig/Vector.h>
#include <yarp/sig/Matrix.h>
#include <string>
#include <vector>
#include <map>
#include <list>
 
namespace iCub
{
 
namespace skinDynLib
{
 
enum VerbosityLevel { NO_VERBOSE, VERBOSE, MORE_VERBOSE};
 
// DEFINE BODY PARTS AND SKIN PARTS OF ICUB ROBOT
#ifndef NAO_SKIN
 
// List of the parts of the iCub body
enum BodyPart {
    BODY_PART_UNKNOWN=0, 
    TORSO, HEAD, LEFT_ARM, RIGHT_ARM, LEFT_LEG, RIGHT_LEG, 
    BODY_PART_ALL, LOWER_BODY_PARTS, UPPER_BODY_PARTS, BODY_PART_SIZE
};
const std::string BodyPart_s[] = {
    "unknown_body_part", 
    "torso", "head", "left_arm", "right_arm", "left_leg", "right_leg", 
    "all_body_parts", "lower_body_parts", "upper_body_parts", "body_part_size"
};
 
// List of the parts composing the skin of iCub
enum SkinPart { 
    SKIN_PART_UNKNOWN=0, 
    SKIN_LEFT_HAND, SKIN_LEFT_FOREARM, SKIN_LEFT_UPPER_ARM, 
    SKIN_RIGHT_HAND, SKIN_RIGHT_FOREARM, SKIN_RIGHT_UPPER_ARM, 
    SKIN_FRONT_TORSO, LEFT_LEG_UPPER, LEFT_LEG_LOWER, LEFT_FOOT,
    RIGHT_LEG_UPPER, RIGHT_LEG_LOWER, RIGHT_FOOT,
    SKIN_PART_ALL, SKIN_PART_SIZE
};
const std::string SkinPart_s[] = {
    "unknown", 
    "l_hand", "l_forearm", "l_upper_arm", 
    "r_hand", "r_forearm", "r_upper_arm", 
    "chest",
    "l_upper_leg", "l_lower_leg", "l_foot",
    "r_upper_leg", "r_lower_leg", "r_foot",
    "all_skin_parts", "skin_part_size"
};
 
// *** Mapping from SKIN PARTs to BODY PARTs ***
// To represent this information we use a vector of struct{SkinPart; BodyPart;}
struct Skin_2_Body { SkinPart skin; BodyPart body; };
const Skin_2_Body SkinPart_2_BodyPart[SKIN_PART_SIZE] = {
    {SKIN_PART_UNKNOWN,     BODY_PART_UNKNOWN}, 
    {SKIN_LEFT_HAND,        LEFT_ARM}, 
    {SKIN_LEFT_FOREARM,     LEFT_ARM}, 
    {SKIN_LEFT_UPPER_ARM,   LEFT_ARM}, 
    {SKIN_RIGHT_HAND,       RIGHT_ARM}, 
    {SKIN_RIGHT_FOREARM,    RIGHT_ARM},
    {SKIN_RIGHT_UPPER_ARM,  RIGHT_ARM}, 
    {SKIN_FRONT_TORSO,      TORSO},
    {SKIN_PART_ALL,         BODY_PART_ALL}
};
 
// association between each skin part and the number of the link where it is mounted
struct Skin_2_Link{ SkinPart skin; int linkNum; };
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE] = {
    {SKIN_PART_UNKNOWN,     -1}, 
    {SKIN_LEFT_HAND,         6}, 
    {SKIN_LEFT_FOREARM,      4}, 
    {SKIN_LEFT_UPPER_ARM,    2}, 
    {SKIN_RIGHT_HAND,        6}, 
    {SKIN_RIGHT_FOREARM,     4},
    {SKIN_RIGHT_UPPER_ARM,   2}, 
    {SKIN_FRONT_TORSO,       2},
    {SKIN_PART_ALL,         -1}
};
#endif
 
// DEFINE BODY PARTS AND SKIN PARTS OF NAO ROBOT
#ifdef NAO_SKIN
// List of the parts of the iCub body
enum BodyPart {
    BODY_PART_UNKNOWN=0, 
    HEAD, TORSO, LEFT_ARM, RIGHT_ARM, LEFT_LEG, RIGHT_LEG, 
    BODY_PART_ALL, BODY_PART_SIZE
};
const std::string BodyPart_s[] = {
    "unknown_body_part", 
    "head", "torso", "left_arm", "right_arm", "left_leg", "right_leg", 
    "all_body_parts", "body_part_size"
};
 
// List of the parts composing the skin of iCub
enum SkinPart { 
    SKIN_PART_UNKNOWN=0, 
    SKIN_LEFT_HAND, SKIN_LEFT_ARM, 
    SKIN_RIGHT_HAND, SKIN_RIGHT_ARM,
    SKIN_PART_ALL, SKIN_PART_SIZE
};
const std::string SkinPart_s[] = {
    "unknown_skin_part", 
    "skin_left_hand", "skin_left_arm",
    "skin_right_hand", "skin_right_arm",
    "all_skin_parts", "skin_part_size"
};
 
// association between each skin part and the corresponding body part
struct Skin_2_Body { SkinPart skin; BodyPart body; };
const Skin_2_Body SkinPart_2_BodyPart[SKIN_PART_SIZE] = {
    {SKIN_PART_UNKNOWN, BODY_PART_UNKNOWN}, 
    {SKIN_LEFT_HAND,    LEFT_ARM}, 
    {SKIN_LEFT_ARM,     LEFT_ARM}, 
    {SKIN_RIGHT_HAND,   RIGHT_ARM}, 
    {SKIN_RIGHT_ARM,    RIGHT_ARM},
    {SKIN_PART_ALL,     BODY_PART_ALL}
};
 
// association between each skin part and the number of the link where it is mounted
struct Skin_2_Link{ SkinPart skin; int linkNum; };
const Skin_2_Link SkinPart_2_LinkNum[SKIN_PART_SIZE] = {
    {SKIN_PART_UNKNOWN, -1}, 
    {SKIN_LEFT_HAND,    -1}, 
    {SKIN_LEFT_ARM,     -1}, 
    {SKIN_RIGHT_HAND,   -1}, 
    {SKIN_RIGHT_ARM,    -1},
    {SKIN_PART_ALL,     -1}
};
#endif
 
enum HandPart { 
    INDEX, MIDDLE, RING, LITTLE, THUMB, PALM, 
    ALL_HAND_PARTS, HAND_PART_SIZE
};
const std::string HandPart_s[] = {
    "index", "middle", "ring", "little", "thumb", "palm", 
    "all_hand_parts", "hand_part_size"
};
 
static const int ARM_FT_SENSOR_LINK_INDEX = 2;  // index of the link containing the F/T sensor in the iCub arms
static const int LEG_FT_SENSOR_LINK_INDEX = 1;  // index of the link containing the F/T sensor in the iCub legs
 
static const int ARM_DOF = 7;
static const int TORSO_DOF = 3;
 
BodyPart getBodyPart(SkinPart s);
 
std::vector<SkinPart> getSkinParts(BodyPart b);
 
int getLinkNum(SkinPart s);
 
SkinPart getSkinPartFromString(const std::string skinPartString);
 
 
 
yarp::sig::Vector toVector(yarp::sig::Matrix m);
 
yarp::sig::Vector vectorFromBottle(const yarp::os::Bottle b, int in, const int size);
 
void vectorIntoBottle(const yarp::sig::Vector v, yarp::os::Bottle &b);
 
yarp::sig::Matrix matrixFromBottle(const yarp::os::Bottle b, int in, const int r, const int c);
 
void      matrixIntoBottle(const yarp::sig::Matrix m, yarp::os::Bottle &b);
 
inline std::list<unsigned int> vectorofIntEqualto(const std::vector<int> vec, const int val)
{
    std::list<unsigned int> res;
    for (size_t i = 0; i < vec.size(); i++)
    {
        if (vec[i]==val)
        {
            res.push_back(i);
        }
    }
    return res;
};
 
}
 
}//end namespace
 
#endif