Tactile sensors (aka Skin)

From Wiki for iCub and Friends
Jump to: navigation, search

Tactile Sensor

Tactile sensor triangle

This page collects information regarding the tactile sensors of iCub.

Physical Sensors

The tactile sensor network (aka skin) of iCub is organized into patches. A patch is a set of tactile sensors that are physically connected one to the other and that are read by the same microcontroller. In particular, at the moment of writing (i.e. 7/10/2013), 18 patches are mounted on the robot:

  • 1 in each hand
  • 2 in each forearm
  • 4 in each upper arm
  • 4 in the torso

Each patch is composed by (at most 16) triangular modules, each consisting of 10 tactile sensors (aka taxels), with two additional small ones that are thermal pads (7th and 11th element). The 4th value corresponds to the taxel in the center of the triangle. (Note: older skin version contained 12 taxels per triangle.)

Reading Raw Tactile Data

Each sensor outputs an integer value in [0 255]: the lower the value, the higher the pressure. The output value with zero pressure may vary with the version of the firmware, but it is typically 235. Tactile sensor data are provided to the user through YARP ports streaming YARP vectors. In particular, at the moment of writing, these are the available ports:

  • /icub/skin/left_hand (1 patch)
  • /icub/skin/left_forearm (2 patches)
  • /icub/skin/left_arm (4 patches)
  • /icub/skin/right_hand (1 patch)
  • /icub/skin/right_forearm (2 patches)
  • /icub/skin/right_arm (4 patches)
  • /icub/skin/torso (4 patches)

Taking a look at the number of ports (i.e. 7) and the number of patches (i.e. 18), it is clear that some ports correspond to more than one patch. Consequently, the sizes of the output vectors of the ports are different. Given that each patch is composed by at most 16 triangles, each containing 12 sensors, each patch can have at most 192 sensors. Hence, each port outputs 192 values for each patch. However, since some patches do not contain 16 triangles (e.g. hand, upper forearm, top upper arm), some of the port output values are always zero - see below for more details.

Reading Compensated Tactile Data

For most applications, raw sensor data need some filtering before being used. The software module skinManager takes care of i) compensating for the thermal drift affecting the sensors and ii) filtering high frequency noise. The configuration files of the skinManager are located in $ICUB_ROOT\main\app\skinGui\conf. The filtered tactile data can be read from YARP ports (similarly to the raw data):

  • /icub/skin/left_hand_comp
  • /icub/skin/left_forearm_comp
  • /icub/skin/left_arm_comp
  • /icub/skin/right_hand_comp
  • /icub/skin/right_forearm_comp
  • /icub/skin/right_arm_comp
  • /icub/skin/torso_comp

Differently from the raw data, these values are floating point numbers in [0 255], where 0 means no pressure.

One can read the output of the skin ports on the terminal using the following command:

  • yarp read ... port_name

Each port outputs a vector of floating point numbers, with output from individual taxels and is zero padded in certain positions. The size of the vector depends on the skin part, in particular it is:

  • 192 for hand data, where 1-60 are taxels of fingertips (12 each in this order: index, middle, ring, little, thumb); 61-96 zeros; 97-144 palm taxels (inside these, 108, 120, 132, and 140 are thermal pads ~ 0s); 145-192 zeros.

With the exception of the palm and finger data, the skin is composed of triangular modules with 10 taxels each. However, there are 12 positions reserved for each module in the port output. The 7th and 11th value are not meaningful as tactile output (signal from thermal pads) and can be ignored.

  • 2*192=384 for forearm data, where first 192 values correspond to a full patch - bottom of forearm. They are 16 consecutive modules. Then, there is the other, incomplete patch, with only 7 modules (skin version 1) / 8 modules (V2) at the top of the forearm. For V1 skin, the positions 193-204; 217-252; 265-288; 325-336; 361-384 are empty. For V2 skin, the empty ones are: 217-228; 241-264; 277-288; 313-336; 361-384.
  • 4*192=768 for (upper) arm data and torso.

Please note that in the text above, we referred to the "positions" on the port starting from 1; however, if you use taxelIDs from the skinDynLin library, they are starting from 0 - that is taxel ID 10, for example, would be 11th on the port. This is also used in the skin_events output (see below).

For every skin part, the coordinates of every taxel in local link reference frames can be retrieved from $ICUB_ROOT\main\app\skinGui\conf\positions - first three columns are x,y,z coordinates. The default output rate is 50 Hz. To save the skin output for post processing one can use the dataDumper software module. Note that the dataDumper saves the output of the port to which it is connected, and it adds two values at the beginning of each sample. The first value is just a counter, whereas the second value is the sample timestamp. For instance, to dump the output of the left forearm skin one can use the following commands:

  • dataDumper --name /dumpSkin
  • yarp connect /icub/skin/left_forearm_comp /dumpSkin

Reading High Level Contact Data

The software module skinManager provides a high-level representation of the tactile data, under the form of a skinContactList. A skinContactList is a list of skinContact. Note that in case multiple tactile sensors are activated by a contact, the skinManager groups them together into a unique skinContact. To do that, skinManager needs to know the distances between the sensors (the files containing the 3D positions of the tactile sensors are located in $ICUB_ROOT\main\app\skinGui\conf\positions). The port on which the skinContactList is written is:

  • /skinManager/skin_events:o

One can read the output of the above mentioned port from the command line using the following command:

  • yarp read ... /skinManager/skin_events:o

Every skinContactList is represented with the following format: (SKIN_CONTACT_VECTOR_1) ... (SKIN_CONTACT_VECTOR_N). There are as many SKIN_CONTACT_VECTORs as there were (clusters of) contacts detected on the whole skin of the robot by the skinManager. If case of no contact, the skinContactList is empty. If there was contact, every SKIN_CONTACT_VECTOR is enclosed by brackets and has the following format: ((contactId bodyPartId linkNumber skinPart) (centerOfPressure_x cOP_y cOP_z) (force_x f_y f_z) (moment_x m_y m_z) (geometricCenter_x gC_y gC_z) (surfaceNormalDirection_x sND_y sND_z) (activatedTaxelId1 aTId2 .. aTIdN) average_pressure). Here more information on some of the data:

  • bodyPart: the part of the body (TORSO=1, HEAD=2, LEFT_ARM=3, RIGHT_ARM=4, LEFT_LEG=5, RIGHT_LEG=6)
  • linkNumber: the link number relative to the specified body part (e.g. upper arm=2, forearm=4, hand=6)
  • skinPart: the part of the skin (SKIN_LEFT_HAND=1, SKIN_LEFT_FOREARM=2, SKIN_LEFT_UPPER_ARM=3, SKIN_RIGHT_HAND=4, SKIN_RIGHT_FOREARM=5, SKIN_RIGHT_UPPER_ARM=6, SKIN_FRONT_TORSO=7)
  • CoP: the center of pressure (expressed in link reference frame)
  • force: force applied at contact (expressed in link reference frame)
  • moment: moment applied at contact (expressed in link reference frame)
  • geoCenter: the geometric center of the contact area (expressed in link reference frame)
  • normalDir: normal direction of the contact area (expressed in link reference frame)
  • taxelList: list of ids of the activated taxels
  • pressure: average output of the activated taxels

Visualizing Tactile Data

At the moment of writing, there are two ways to visualize tactile data:

  1. iCubSkinGui
  2. icubGui

The iCubSkinGui visualizes a 2D representation of one or more patches of tactile sensors. The GUI reads the tactile data from an input YARP port, hence it can visualize both raw data and compensated data. The configuration files for this GUI are located in $ICUB_ROOT\main\app\skinGui\conf\skinGui.

The icubGui visualizes a 3D model of the iCub. To visualize the tactile data with this GUI you need to launch the skinManager module and connect the skinManager output port /skinManager/skin_events:o to the icubGui input port /iCubGui/forces. Then, this GUI visualizes a red arrow for each contact detected by the tactile sensors. The length of the arrow is proportional to the sum of the outputs of the tactile sensors.

Alternatively, if the robot is equipped with force/torque sensors and wholeBodyDynamics is running, the icubGui can be connected to the port /wholeBodyDynamics/contacts:o. In this case the tactile data are integrated with the force/torque data to estimate the external contact forces. This means that the length of the arrows displayed by the icubGui would be proportional to the estimated force magnitude (rather than the tactile sensor outputs). To make this system work you need to connect the skinManager port /skinManager/skin_events:o to the wholeBodyDynamics port /wholeBodyDynamics/skin_contacts:i.

Tactile Data Flow

Skin_yarp_interface_and_tools.png

This first picture depicts the flow of the tactile data, starting from the physical sensors and moving up to the user applications and the GUIs. The tactile sensor data are read from the iCubInterface (on the PC104, located in iCub's head) and sent on YARP ports with names /icub/skin/part_name ("data ports" in the figure). There is also one "rpc port" for each skin "data port", which are used to send a reset command to the skin microcontrollers. The skin data port can then be read either directly, or through the IAnalogSensor YARP interface (as skinManager does). SkinManager reads the raw tactile data and creates another set of YARP ports with names /icub/skin/part_name_comp, which stream the filtered data (after thermal drift compensation and high frequency noise filtering). Moreover, SkinManager also creates one "event port" that outputs a skinContactList, that is a high-level representation of the contacts detected by the skin (i.e. it abstracts from the specific sensor configuration and it provides information such as contact center of pressure, contact force, contact link). The skinGui is typically connected to the ports outputting the filtered tactile data. SkinManager has its own GUI, which allows the user to adjust the filtering parameters, switch the visualization from continuous to binary (i.e. contact/noncontact) and to monitor the sensor state.

Scheme of tactile data flow for autonomous skin calibration

The second picture depicts a scheme of a possible application that uses the tactile data. In particular, the depicted application is the autonomous skin calibration, which is composed by two main modules:

  • Parallel Control: module controlling the robot to make it interact with the environment
  • Skin Calibrator: module estimating online the 3d positions of the tactile sensors

Note how the tactile data are displayed by both the skinGui and the iCubGui.