Getting Sensor Data

[BlendingInfinite]

Hello,
as I have understood gazebo-yarp gets only the torque/force sensors of e.g. the arms,feets and legs, is that correct? We now want to simulate the skin in gazebo. So we have to add manually new sensors in the gazebo model? Our question is now how to do get the skin data from gazebo best after we have added some new sensors (create an own interface or adapt the yarp plugin?).

Greetings,
Moritz

[traversaro]

Hi @MoritzN89, simulation of skin output is for sure a topic we are interested in (see #55 ), but unfortunately we didn't had the time to work on it.

In a ideal scenario, you should add the "skin" sensor to Gazebo, and then write a gazebo plugin that implements a yarp driver for the simulated skin sensor. Unfortunately there is no "skin" sensor in Gazebo, so we should find a way for simulating taxel outputs. For simulating taxel outputs I can think of two strategies:

• Model each taxel as a physically compliant (eventually nonlinear) element. This is the strategy is the one used by the guys developing SkinSim.
• Get the external force applied to a link from Gazebo, and then estimate the possible output of a skin taxel, as a function of distance of the application point from the taxel and of the force applied.

The first approach could lead to more physically accurate outputs, but is much more complex both in implementation and for simulation computation time. However the second method is much more simpler and should be ok for most of the use cases. Which kind of application you have in mind for skin simulation?

[BlendingInfinite]

Thank you for the answer. We need the skin data for improving the balancing of the iCub. So we want to couple it with the balancing controller.

[BlendingInfinite]

We now decided to use one approach. To fulfill that we have to create own blocks in the Simulink library for our own sensors. I watched at the masks of the provided bocks but dont understand it completely. So how can we do that?

[traversaro]

I don't get how you decided to publish the sensor information to YARP from
Gazebo, anyway the easiest block to read data from a YARP port in Simulink
is the yarpRead block.
On Jan 24, 2015 8:47 PM, "MoritzN89" <[email protected]
javascript:_e(%7B%7D,'cvml','[email protected]');> wrote:

So we decided to one approach. We now want to proceed and create own
simulink blocks in the provided library for our own sensors. As we found
out the file to edit is the WBCLibrary.mdl. The file is very confusing and
it seems that it was created with an own tool.
So how can we make own blocks which are communicating with yarp for our
sensors?

—
Reply to this email directly or view it on GitHub
#168 (comment)
.

[BlendingInfinite]

I want to create a new sensor on the icub.sdf and put the force/torque plugin in there. Then I want to read it in Simulink and get the outputs, if that can be done that easily. I tried to use the yarp read block on a existing torque sensor for testing but I get no output, maybe because I have to replace something in the 'To' parameter like on the picture attached? But I dont know what to replace.

[traversaro]

... in YARP is the shorthand to a randomly created port used just for reading data from the port that you put in the From form.
To debug your problem, can you try writing in a terminal:

yarp read ... /icubGazeboSim/torso/analog:o/torques 

To see if you properly read something?
You can also use yarp name list to see all the ports registered in your YARP network.

[BlendingInfinite]

I get no output from the yarp read command, yarp cant find the port. This is the output for yarp name list:

registration name /icubGazeboSim/applyExternalWrench/rpc:i ip 130.83.116.6 port 10039 type tcp
registration name /icubGazeboSim/head/command:i ip 130.83.116.6 port 10020 type tcp
registration name /icubGazeboSim/head/rpc:i ip 130.83.116.6 port 10019 type tcp
registration name /icubGazeboSim/head/state:o ip 130.83.116.6 port 10021 type tcp
registration name /icubGazeboSim/head/stateExt:o ip 130.83.116.6 port 10022 type tcp
registration name /icubGazeboSim/inertial ip 130.83.116.6 port 10002 type tcp
registration name /icubGazeboSim/left_arm/analog:o ip 130.83.116.6 port 10012 type tcp
registration name /icubGazeboSim/left_arm/analog:o/rpc:i ip 130.83.116.6 port 10011 type tcp
registration name /icubGazeboSim/left_arm/command:i ip 130.83.116.6 port 10028 type tcp
registration name /icubGazeboSim/left_arm/rpc:i ip 130.83.116.6 port 10027 type tcp
registration name /icubGazeboSim/left_arm/state:o ip 130.83.116.6 port 10029 type tcp
registration name /icubGazeboSim/left_arm/stateExt:o ip 130.83.116.6 port 10030 type tcp
registration name /icubGazeboSim/left_foot/analog:o ip 130.83.116.6 port 10006 type tcp
registration name /icubGazeboSim/left_foot/analog:o/rpc:i ip 130.83.116.6 port 10005 type tcp
registration name /icubGazeboSim/left_leg/analog:o ip 130.83.116.6 port 10004 type tcp
registration name /icubGazeboSim/left_leg/analog:o/rpc:i ip 130.83.116.6 port 10003 type tcp
registration name /icubGazeboSim/left_leg/command:i ip 130.83.116.6 port 10036 type tcp
registration name /icubGazeboSim/left_leg/rpc:i ip 130.83.116.6 port 10035 type tcp
registration name /icubGazeboSim/left_leg/state:o ip 130.83.116.6 port 10037 type tcp
registration name /icubGazeboSim/left_leg/stateExt:o ip 130.83.116.6 port 10038 type tcp
registration name /icubGazeboSim/right_arm/analog:o ip 130.83.116.6 port 10014 type tcp
registration name /icubGazeboSim/right_arm/analog:o/rpc:i ip 130.83.116.6 port 10013 type tcp
registration name /icubGazeboSim/right_arm/command:i ip 130.83.116.6 port 10024 type tcp
registration name /icubGazeboSim/right_arm/rpc:i ip 130.83.116.6 port 10023 type tcp
registration name /icubGazeboSim/right_arm/state:o ip 130.83.116.6 port 10025 type tcp
registration name /icubGazeboSim/right_arm/stateExt:o ip 130.83.116.6 port 10026 type tcp
registration name /icubGazeboSim/right_foot/analog:o ip 130.83.116.6 port 10010 type tcp
registration name /icubGazeboSim/right_foot/analog:o/rpc:i ip 130.83.116.6 port 10009 type tcp
registration name /icubGazeboSim/right_leg/analog:o ip 130.83.116.6 port 10008 type tcp
registration name /icubGazeboSim/right_leg/analog:o/rpc:i ip 130.83.116.6 port 10007 type tcp
registration name /icubGazeboSim/right_leg/command:i ip 130.83.116.6 port 10032 type tcp
registration name /icubGazeboSim/right_leg/rpc:i ip 130.83.116.6 port 10031 type tcp
registration name /icubGazeboSim/right_leg/state:o ip 130.83.116.6 port 10033 type tcp
registration name /icubGazeboSim/right_leg/stateExt:o ip 130.83.116.6 port 10034 type tcp
registration name /icubGazeboSim/torso/command:i ip 130.83.116.6 port 10016 type tcp
registration name /icubGazeboSim/torso/rpc:i ip 130.83.116.6 port 10015 type tcp
registration name /icubGazeboSim/torso/state:o ip 130.83.116.6 port 10017 type tcp
registration name /icubGazeboSim/torso/stateExt:o ip 130.83.116.6 port 10018 type tcp
registration name /root ip 130.83.116.6 port 10000 type tcp
registration name fallback ip 224.2.1.1 port 10000 type mcast
*** end of message

[traversaro]

Sorry, which kind of sensor are you trying to read? Joint torques?

[BlendingInfinite]

I want to read the joint torques

[traversaro]

Joint torque information is published by the gazebo_yarp_controlboard plugin, in a stateExt:o port that will publish all the joint information following this format: https://github.com/robotology/yarp/blob/4698c64a8190ff463bad82eac9013f0c61480caf/src/libYARP_dev/src/modules/msgs/stateExt.thrift .
You can easily read this kind information in WBI-Toolbox using the readTorques block. Otherwise you can read the stateExt:o port, but that port cannot be read directly from the yarpRead block because it is processing elements that are not simple vectors.

[BlendingInfinite]

I tried it but I dont find the ports on stateExt:o for torques, only for example /icubGazeboSim/torso/stateExt:o. If I now want to create own force and torque sensors on a own sensor (e.g. a block around a link or a link itself) how can I proceed? How can I add an own port in yarp and send the data correctly from gazebo to yarp?

[traversaro]

Ok, I am afraid we are confusing two different sensors:
In the stateExt:o we can find joint torque measurements (or estimations): for each joint we measure/estimate the torque at each one of the joints of that part.
So for example the /icubGazeboSim/torso/stateExt:o outputs the torques of the three joints of the torso.

The force_torque sensors instead simulated a 6-Axis Force Torque sensors, that is measuring the force/torque exchanged between two bodies. Depending on the version the iCub can be equipped with up to 6 Force/Torque sensors.

[BlendingInfinite]

Ok so what is if we want to e.g. place several hundreds of force_torque sensors on the iCub, is there a way to do it?

[traversaro]

Please note (for example from SDF documentation: http://sdformat.org/spec?ver=1.5&elem=sensor#sensor_force_torque ) that the force torque sensors is just reading the force/torque transmitted through a joint (that is connecting two links). What do you mean by "place several hundreds of force_torque sensors on the iCub"?

[robertocalandra]

I guess that he means skin sensors, not FTS.

[traversaro]

Skin taxel simulation is an open issue (#55) in gazebo-yarp-plugins, for more information check my first comment: #168 (comment) .

[traversaro]

Closing, further discussion for integration of @MoritzN89 code can be done in #55 .