robotology-legacy / wbi-toolbox-controllers Goto Github PK

[WARNING]: this repository is no more mantained. Please refer to https://github.com/robotology/whole-body-controllers.

CMake 0.04% MATLAB 99.93% M 0.03%

wbi-toolbox-controllers's Introduction

Warning: this repository is not mantained

The Simulink controllers contained in the repo are not supposed to work. This repository is currently used as legacy of whole-body-controllers, see also https://github.com/robotology/whole-body-controllers/blob/master/README.md.

WBI-Toolbox-controllers

Controllers developed using both the WBI-Toolbox and the WB-Toolbox. In the folder which correspond to each controller the user can find a detailed README. Here we present a short description of each balancing controller and the README links.

Available models

All the models implemented with the old WBI-Toolbox are in the legacy folder. The new WB-Toolbox models are:

boundedTorqueControl
torqueBalancing
legacy
torqueBalancingOnSeesaw
torqueBalancingStandUp
wholeBodyImpedanceControl

Check the documentation in WB-Toolbox for installation of the Simulink toolbox.

torqueBalancing

This simulink model controls the robot using a momentum-based balancing control. The user can find the full description here: torqueBalancing README

boundedTorqueControl

legacy

A folder containing old Simulink controllers for balancing.

torqueBalancingOnSeesaw

This is a peculiar version of momentum-based balancing controller, specifically developed for balancing the robot on a seesaw. The readme is at the following link: torqueBalancingOnSeesaw README.

torqueBalancingStandUp

Another peculiar version of momentum-based balancing controller designed for standing up from a chair. See relative torqueBalancingStandUp README.

wholeBodyImpedanceControl

A simple impedance controller for the robot balancing.

Add a new model

To add a new model to the repo, please follow the guidelines on model creation.

wbi-toolbox-controllers's People

Contributors

Stargazers

Watchers

Forkers

s-dafarra diegoferigo pi-q aikolina

wbi-toolbox-controllers's Issues

WBToolbox: InverseKinematics box cannot retrieve string from robot part parameter

Hi, we are trying to implement a reactive walking controller on iCub2 at EPFL LASA, and to do so we are trying to combine it with the matlab torqueBalancing module by exploiting the ComDes:i and the qDes:i input ports. However in our implementation we tried to add an inverse Kinematics box from the WBT_toolbox in order to compute the qDes and it keeps on giving the following error msg:
Error:Error reported by S-function 'WBToolbox' in 'torqueBalancingAndWalking/Inverse Kinematics1/S-Function': [mdlStart] Cannot retrieve string from robot part parameter.

Here are the configuration parameters during the compilation:

[INFO][robotStatus::robotConfig] Configuring...
[INFO][robotStatus::robotConfig] wbiConfFileFullPath is: /home/michael/icub/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][robotStatus::robotConfig] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][robotStatus::robotConfig] robot_fixed is false!
[DEBUG][robotStatus::robotConfig] After reading from config file, params are
[DEBUG][robotStatus::robotConfig] robot name: icubGazeboSim
[DEBUG][robotStatus::robotConfig] local name: matlabTorqueBalancing
[DEBUG][robotStatus::robotConfig] world reference frame: l_sole
[DEBUG][robotStatus::robotConfig] WBI ID List: ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP

Please, can anyone tell us what is wrong and how we can fix it?

Thanks in advance.

To install or not to install?

Before definitely removing the /controllers directory from the WBI-Toolbox and using this repo instead, I was wondering:

Do you agree we should install these controllers❓ Currently, if you make install this project it will copy all the controllers and simulink models to ${CODYCO_SUPERBUILD_DIR}/install/share/WBI-Toolbox-controllers/controllers
What about adding it to Matlab's path when installing the WBI-Toolbox❓
cc @traversaro @DanielePucci @francesco-romano @iron76

Please vote:
👍 for yes, 👎 for no

Write a description of each controller in the main README

And also add a link to the README of each controller.
See #11 (comment) .

iCub stand up demo general update

I updated the code for iCub stand up demo. In particular, there are now 2 more branches in WBIToolbox-controllers:

icub_chair is the old branch, and it will be removed after testing the new code on iCub.
icub_standUp it substitutes icub_chair branch. I did a bit of cleanup of the code. Please note that now the name of the iCub stand up demo is no more "CHAIR" but "STANDUP".
icub_standUp_4Contacts in this branch, iCub uses 4 contacts with the ground (both legs and feet) for balancing on the chair. Nothing else is changed.

Furthermore, also icub-gazebo-wholebody repository has been updated. Note that the iCub model for sitting on the chair is renamed icub_standup and the world name is now icub_standup_world. (see also robotology/icub-gazebo-wholebody#9)

Occasional Matlab crash when initializing torqueBalancing

Here the dump

------------------------------------------------------------------------
          std::terminate() detected at Mon Aug 22 14:36:58 2016
------------------------------------------------------------------------

Configuration:
  Crash Decoding      : Disabled
  Crash Mode          : continue (default)
  Current Graphics Driver: Intel Open Source Technology Center Mesa DRI Intel(R) HD Graphics 5500 (Broadwell GT2)  Version 3.0 Mesa 10.5.9
  Current Visual      : 0x20 (class 4, depth 24)
  Default Encoding    : UTF-8
  GNU C Library       : 2.19 stable
  Host Name           : iiticublap042
  MATLAB Architecture : glnxa64
  MATLAB Root         : /usr/local/MATLAB/R2016a
  MATLAB Version      : 9.0.0.341360 (R2016a)
  OpenGL              : hardware
  Operating System    : Linux 3.19.0-66-generic #74~14.04.1-Ubuntu SMP Tue Jul 19 19:56:11 UTC 2016 x86_64
  Processor ID        : x86 Family 6 Model 61 Stepping 4, GenuineIntel
  Virtual Machine     : Java 1.7.0_60-b19 with Oracle Corporation Java HotSpot(TM) 64-Bit Server VM mixed mode
  Window System       : The X.Org Foundation (11701000), display :0

Fault Count: 1


Abnormal termination:
std::terminate()

Register State (captured):
  RAX = 0000000000000020  RBX = 00007f72a890a7e8
  RCX = 0000000000000002  RDX = 00007f72a88fac20
  RSP = 00007f70ca7edff0  RBP = 00007f70ca7ee3b0
  RSI = 00007f72a86b13b8  RDI = 00007f70ca7ee000

   R8 = 0000000000000000   R9 = 00007f72ad309fb0
  R10 = 00007f72ac6a887e  R11 = 00007f72ac6a887f
  R12 = 00007f72a891a878  R13 = 00007f70ca7ef070
  R14 = 00007f70ca7ef050  R15 = 00007f70ca7ef430

  RIP = 00007f72a861e59e  EFL = 0000000000000002

   CS = 0001   FS = 0000   GS = 0000

Stack Trace (captured):
[  0] 0x00007f72a861e59e    /usr/local/MATLAB/R2016a/bin/glnxa64/libmwfl.so+01082782 _ZN2fl4diag5linux6x86_6412context_base12capture_dataEv+00000030
[  1] 0x00007f72a861ae0a    /usr/local/MATLAB/R2016a/bin/glnxa64/libmwfl.so+01068554
[  2] 0x00007f729d5771e5   /usr/local/MATLAB/R2016a/bin/glnxa64/libmwmcr.so+00913893
[  3] 0x00007f729d578fa0   /usr/local/MATLAB/R2016a/bin/glnxa64/libmwmcr.so+00921504
[  4] 0x00007f729d579088   /usr/local/MATLAB/R2016a/bin/glnxa64/libmwmcr.so+00921736
[  5] 0x00007f729d579453   /usr/local/MATLAB/R2016a/bin/glnxa64/libmwmcr.so+00922707
[  6] 0x00007f729d5794c3   /usr/local/MATLAB/R2016a/bin/glnxa64/libmwmcr.so+00922819
[  7] 0x00007f72ad0806d6           /usr/lib/x86_64-linux-gnu/libstdc++.so.6+00386774
[  8] 0x00007f72ad080703           /usr/lib/x86_64-linux-gnu/libstdc++.so.6+00386819
[  9] 0x00007f72ad080922           /usr/lib/x86_64-linux-gnu/libstdc++.so.6+00387362
[ 10] 0x00007f72ad080e0d           /usr/lib/x86_64-linux-gnu/libstdc++.so.6+00388621 _Znwm+00000125
[ 11] 0x00007f7178fe4311 /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_dev.so.1+00877329
[ 12] 0x00007f7178feac24 /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_dev.so.1+00904228
[ 13] 0x00007f7178fe26c4 /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_dev.so.1+00870084
[ 14] 0x00007f717b8350a1 /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_OS.so.1+01073313 _ZN4yarp2os4impl20PortReaderBufferBase4readERNS0_16ConnectionReaderE+00000337
[ 15] 0x00007f717b833e46 /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_OS.so.1+01068614
[ 16] 0x00007f717b814afe /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_OS.so.1+00940798 _ZN4yarp2os4impl8PortCore9readBlockERNS0_16ConnectionReaderEPvPNS0_12OutputStreamE+00000158
[ 17] 0x00007f717b822a9a /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_OS.so.1+00998042
[ 18] 0x00007f717b892995 /home/sdafarra/codyco-superbuild/build/install/mex/../lib/libYARP_OS.so.1+01456533
[ 19] 0x00007f7179c50b16                           /usr/lib/libACE-6.0.3.so+00940822 _ZN21ACE_OS_Thread_Adapter6invokeEv+00000134
[ 20] 0x00007f72ac8f0184              /lib/x86_64-linux-gnu/libpthread.so.0+00033156
[ 21] 0x00007f72ac61d37d                    /lib/x86_64-linux-gnu/libc.so.6+01024893 clone+00000109
[ 22] 0x0000000000000000                                   <unknown-module>+00000000


If this problem is reproducible, please submit a Service Request via:
    http://www.mathworks.com/support/contact_us/

A technical support engineer might contact you with further information.

Thank you for your help.

Before the crash, on the corresponding terminal, these were the last lines:

||| finding file [model.urdf]
||| checking [/home/sdafarra/codyco-superbuild/main/WBIToolboxControllers/controllers/torqueBalancing/model.urdf] (pwd)
||| checking [/home/sdafarra/.config/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_HOME)
||| checking [/home/sdafarra/.local/share/yarp/robots/icubGazeboSim] (robot YARP_DATA_HOME)
||| found /home/sdafarra/.local/share/yarp/robots/icubGazeboSim
||| checking [/etc/xdg/xdg-ubuntu/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/usr/share/upstart/xdg/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/etc/xdg/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| checking [/home/sdafarra/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| found /home/sdafarra/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim
||| checking [config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/home/sdafarra/codyco-superbuild/build/install/share/codyco/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/home/sdafarra/.local/share/yarp/robots/icubGazeboSim/model.urdf] (robot)
||| checking [/home/sdafarra/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/model.urdf] (robot)
||| found /home/sdafarra/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/model.urdf
yarp: Port /tmp/port/12 active at tcp://127.0.0.1:10154
yarp: Port /tmp/port/13 active at tcp://127.0.0.1:10155
yarp: Port /tmp/port/14 active at tcp://127.0.0.1:10156
yarp: Port /tmp/port/15 active at tcp://127.0.0.1:10157
yarp: Port /matlabTorqueBalancing/comDes:i active at tcp://127.0.0.1:10147
yarp: Port /matlabTorqueBalancing/qDes:i active at tcp://127.0.0.1:10148
yarp: Port /matlabTorqueBalancing/constraints:i active at tcp://127.0.0.1:10149
yarp: Port /tmp/port/16 active at tcp://127.0.0.1:10158
yarp: Receiving input from /wholeBodyDynamicsTree/right_foot/cartesianEndEffectorWrench:o to /tmp/port/16 using tcp
yarp: Port /tmp/port/17 active at tcp://127.0.0.1:10159
yarp: Receiving input from /wholeBodyDynamicsTree/left_foot/cartesianEndEffectorWrench:o to /tmp/port/17 using tcp
yarp: Port /tmp/port/18 active at tcp://127.0.0.1:10160
yarp: Receiving input from /icubGazeboSim/inertial to /tmp/port/18 using tcp
yarp: Port /tmp/port/19 active at tcp://127.0.0.1:10161
yarp: Port /tmp/port/2 active at tcp://127.0.0.1:10141
yarp: Port /tmp/port/20 active at tcp://127.0.0.1:10162
yarp: Receiving input from /wholeBodyDynamicsTree/left_foot/cartesianEndEffectorWrench:o to /tmp/port/20 using tcp
yarp: Port /tmp/port/21 active at tcp://127.0.0.1:10163
yarp: Receiving input from /wholeBodyDynamicsTree/right_foot/cartesianEndEffectorWrench:o to /tmp/port/21 using tcp
yarp: Port /tmp/port/22 active at tcp://127.0.0.1:10164
yarp: Receiving input from /wholeBodyDynamicsTree/torso/cartesianEndEffectorWrench:o to /tmp/port/22 using tcp
yarp: Port /tmp/port/23 active at tcp://127.0.0.1:10165
yarp: Receiving input from /icubGazeboSim/inertial to /tmp/port/23 using tcp
yarp: Port /tmp/port/24 active at tcp://127.0.0.1:10166
yarp: Port /tmp/port/3 active at tcp://127.0.0.1:10142
yarp: Port /tmp/port/4 active at tcp://127.0.0.1:10143
yarp: Port /tmp/port/5 active at tcp://127.0.0.1:10144
yarp: Port /tmp/port/6 active at tcp://127.0.0.1:10145
yarp: Receiving input from /wholeBodyDynamicsTree/left_foot/cartesianEndEffectorWrench:o to /tmp/port/6 using tcp
yarp: Port /tmp/port/7 active at tcp://127.0.0.1:10146
yarp: Port /tmp/port/8 active at tcp://127.0.0.1:10150
yarp: Receiving input from /icubGazeboSim/inertial to /tmp/port/8 using tcp
yarp: Port /tmp/port/9 active at tcp://127.0.0.1:10151
yarp: Port /model/torso/rpc:o active at tcp://127.0.0.1:10167
yarp: Port /model/torso/command:o active at tcp://127.0.0.1:10168
yarp: Port /model/torso/state:i active at tcp://127.0.0.1:10169
yarp: Port /model/torso/stateExt:i active at tcp://127.0.0.1:10170
yarp: Sending output from /model/torso/rpc:o to /icubGazeboSim/torso/rpc:i using tcp
yarp: Sending output from /model/torso/command:o to /icubGazeboSim/torso/command:i using udp
yarp: Receiving input from /icubGazeboSim/torso/state:o to /model/torso/state:i using udp
yarp: Receiving input from /icubGazeboSim/torso/stateExt:o to /model/torso/stateExt:i using udp
yarp: Port /model/left_arm/rpc:o active at tcp://127.0.0.1:10171
yarp: Port /model/left_arm/command:o active at tcp://127.0.0.1:10172
yarp: Port /model/left_arm/state:i active at tcp://127.0.0.1:10173
yarp: Port /model/left_arm/stateExt:i active at tcp://127.0.0.1:10174
yarp: Sending output from /model/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i using tcp
yarp: Sending output from /model/left_arm/command:o to /icubGazeboSim/left_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/state:o to /model/left_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/stateExt:o to /model/left_arm/stateExt:i using udp
yarp: Port /model/right_arm/rpc:o active at tcp://127.0.0.1:10175
yarp: Port /model/right_arm/command:o active at tcp://127.0.0.1:10176
yarp: Port /model/right_arm/state:i active at tcp://127.0.0.1:10177
yarp: Port /model/right_arm/stateExt:i active at tcp://127.0.0.1:10178
yarp: Sending output from /model/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i using tcp
yarp: Sending output from /model/right_arm/command:o to /icubGazeboSim/right_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/state:o to /model/right_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/stateExt:o to /model/right_arm/stateExt:i using udp
yarp: Port /model/left_leg/rpc:o active at tcp://127.0.0.1:10179
yarp: Port /model/left_leg/command:o active at tcp://127.0.0.1:10180
yarp: Port /model/left_leg/state:i active at tcp://127.0.0.1:10181
yarp: Port /model/left_leg/stateExt:i active at tcp://127.0.0.1:10182
yarp: Sending output from /model/left_leg/rpc:o to /icubGazeboSim/left_leg/rpc:i using tcp
yarp: Sending output from /model/left_leg/command:o to /icubGazeboSim/left_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/state:o to /model/left_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/stateExt:o to /model/left_leg/stateExt:i using udp
yarp: Port /model/right_leg/rpc:o active at tcp://127.0.0.1:10183
yarp: Port /model/right_leg/command:o active at tcp://127.0.0.1:10184
yarp: Port /model/right_leg/state:i active at tcp://127.0.0.1:10185
yarp: Port /model/right_leg/stateExt:i active at tcp://127.0.0.1:10186
yarp: Sending output from /model/right_leg/rpc:o to /icubGazeboSim/right_leg/rpc:i using tcp
yarp: Sending output from /model/right_leg/command:o to /icubGazeboSim/right_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/state:o to /model/right_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/stateExt:o to /model/right_leg/stateExt:i using udp
yarp: Removing output from /model/torso/rpc:o to /icubGazeboSim/torso/rpc:i
yarp: Removing output from /model/torso/command:o to /icubGazeboSim/torso/command:i
yarp: Removing input from /icubGazeboSim/torso/state:o to /model/torso/state:i
yarp: Removing input from /icubGazeboSim/torso/stateExt:o to /model/torso/stateExt:i
yarp: Removing output from /model/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i
yarp: Removing output from /model/left_arm/command:o to /icubGazeboSim/left_arm/command:i
yarp: Removing input from /icubGazeboSim/left_arm/state:o to /model/left_arm/state:i
yarp: Removing input from /icubGazeboSim/left_arm/stateExt:o to /model/left_arm/stateExt:i
yarp: Removing output from /model/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i
yarp: Removing output from /model/right_arm/command:o to /icubGazeboSim/right_arm/command:i
yarp: Removing input from /icubGazeboSim/right_arm/state:o to /model/right_arm/state:i
yarp: Removing input from /icubGazeboSim/right_arm/stateExt:o to /model/right_arm/stateExt:i
yarp: Removing output from /model/left_leg/rpc:o to /icubGazeboSim/left_leg/rpc:i
yarp: Removing output from /model/left_leg/command:o to /icubGazeboSim/left_leg/command:i
yarp: Removing input from /icubGazeboSim/left_leg/state:o to /model/left_leg/state:i
yarp: Removing input from /icubGazeboSim/left_leg/stateExt:o to /model/left_leg/stateExt:i
yarp: Removing output from /model/right_leg/rpc:o to /icubGazeboSim/right_leg/rpc:i
yarp: Removing output from /model/right_leg/command:o to /icubGazeboSim/right_leg/command:i
yarp: Removing input from /icubGazeboSim/right_leg/state:o to /model/right_leg/state:i

I played a modified version of torqueBalancing, but I think that my modifications doesn't affect "initialization" phase. Anyway it is the first and only time it happened.

Thank you!

[torqueBalancing] Instabilities when using velocities provided by Gazebo

When using the option

readSpeedAccFromControlBoard

in the configuration file yarpWholeBodyInterface.ini, which enables velocity readings from Gazebo and skip the estimation by adaptive filters, the controller is not stable.

MATLAB setenv/getenv does not play nice with YARP's NetworkBase::getEnvironment

NetworkBase::getEnvironment

torqueBalancing controller won't run on Simulink 2012b (8.0)

Since I've updated the controllers through codyco-superbuild, loading the torqueBalancing controller will make Matlab crash systematically. Actually, I'm running Matlab 2012b with the online licenced Simulink Toolbox 2012b. I've just noticed the controller has been saved in Simulink 8.6 (2015b), probably not compatible with Matlab2012b.
@DanielePucci could you please check this compatibility issue, and eventually the possibility to save models in a "compatibility mode"? Thanks

Move this repo to Robotology Legacy?

After properly document it, if everybody agrees.

[torqueBalancing] "Conflicts" between reference generator and torqueBalancing

Hi,
I'm not sure if this issue should go here and if this is a bug or what else, however today we encountered the following problem: we launched the torqueBalancing from the matlab module on pc1, and it was running without problem, then on pc2 we launched another matlab session to run the reference generator, and we had errors.
We stopped the torqueBlancing, and tried to run only the reference generator, and it run without problems.
Then I tried also to run the torqueBalancing from the c++ module, and then the reference generator from matlab, and again we got errors. But if we launch reference generator first, then no errors occur.
With the c++ module the errors appear if the torqueBalancing has been started. If it's just launched without starting from rpc, then the error also do not occur.

So it seemed to me that there were some conflicts, but I don't know how to figure out exactly where the problem is.

@KevinAStein

Transform all .slx controllers into .mdl for back compatibility

It seems that MATLAB 2014a has some troubles in opening the the matlab functions contained in the slx controllers saved with MATLAB 2014b. However, this problem does not arise for the .mdl controllers.

WARNING: matlab does not change some variables after switching branches

I was working in my branch. Then, I switched to another branch without closing matlab. I deleted the slprj folder and I run clear command, but even doing so some variables were not updated, and previous values were kept instead.

Interestingly, this problem affected not only global or persistent variables, but also a matlab structure, sm, that is defined in the Yoga initialization file. In fact, I got an error while compiling the Simulink model because a function was looking for a field in sm that does not exist in the previous branch.

Furthermore, the problem is reproducible.

Differences between ImpedanceControl and ImpedanceControl2012b

A couple of days ago, @Yeshasvitvs noted that ImpedanceControl2012b.mdl assumes the fixed link to be the left foot, and therefore it uses the transformation between left foot and the base to evaluate system's mass matrix:

however, if I remember correctly, this controller is made to be used with the robot on the pole, hence the fixed link should be the base itself. That transformation should be the identity, as actually is inside ImpedanceControl.mdl:

am I right? if so, I can proceed exporting ImpedanceControl.mdl and overwriting ImpedanceControl2012b.mdl

[torqueBalancing] Can the controller introduce self-maintained oscillations?

When performing the Yoga on the real robots it happens sometimes that the robot start shaking while balancing on single support.

Trying to figure out from where these oscillations my arise, we noticed that they come from, we noticed that the desired torques shows well sustained oscillations in one or more joints (for example the torso pitch and the ankle pitch) while the measured shows distorted and less frequent oscillations. This fact may show that indeed these vibrations may not be caused by the saturation of the F/Ts, or that at least they are not the only cause. An additional clue for this lead, is the following screenshot:

It has been taken from a successful (see no high frequency oscillations) Yoga extended demo on both feet. It is possible to notice that those estimated contact wrenches show a saturation (while balancing on the right foot), but the robot was still able to balance.

By further investigation we noticed that the estimated velocity of the base showed a big oscillation at very high frequency, which was then transmitted into the desired contact wrenches by the QP. Any idea on the source of these oscillations and how to get rid of them?

@gabrielenava @DanielePucci @traversaro @fiorisi @fjandrad

Stance leg knee is bending while performing Yoga++ demo

While performing Yoga++ demo on iCubGenova04, the stance leg knee joint started bending. It happened when the robot was on one foot. It seems the problem is related to the configuration defined in the file wholebodydynamics.xml. In particular, the "standard" configuration was not causing the knee to bend, while the configuration used for standing up the robot was.

Here is the result of running diff command between the two configurations (symbol < denotes the stand up configuration while > indicates the standard one):

9c9
<         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,r_upper_leg,l_upper_leg,l_elbow_1,r_elbow_1)</param>
---
>         <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,r_lower_leg,l_lower_leg,l_elbow_1,r_elbow_1)</param>
11a12
>       <param name="forceTorqueEstimateConfidence">2</param>
19a21,22
<             <param name="l_upper_leg">(l_upper_leg,5,2)</param>
29d28
<             <param name="r_upper_leg">(r_upper_leg,6,2)</param>
40c39
<       <!--    <group name="FT_SECONDARY_CALIBRATION">
---
>         <group name="FT_SECONDARY_CALIBRATION">
55c54,55
<           </group> -->
---
>           </group>
>
57,58d56
<             <param name="/wholeBodyDynamics/left_arm/endEffectorWrench:o">(l_hand,l_hand_dh_frame,l_sole)</param>
<             <param name="/wholeBodyDynamics/right_arm/endEffectorWrench:o">(r_hand,r_hand_dh_frame,l_sole)</param>

MATLAB setenv/getenv does not play nice with NetworkBase::getEnvironment

http://www.yarp.it/classyarp_1_1os_1_1NetworkBase.html#a283af07d20c62b6eb9826f9bda68c3f2

Toolbox for exporting Simulink model in previous version automatically

Given the number of different Simulink versions we are using, this can be useful to ensure all users can access the most updated version of the code.

Weird behavior of gravity compensation on icubGazeboSim

I did a simple modification of wholeBodyImpedance:

I took the gravity compensation part. I removed the feedback + feedforward part. I substituted it with a block that can do a sweep in frequency on a sine function. I set the AMPLITUDE of the sine to zero to verify the effectiveness of gravity compensation with icubGazeboSim. This is the result (after 30 [s] of simulation):

It is possible to reproduce the same behavior using wholeBodyImpedance. I did the following modification:

it is the classical wholeBodyImedance controller but I added three GAIN blocks in which I set the gain to zero in order to remove the feedforward/feedback part. This is the result after 30[s]:

and these are the gravity torques:

I know this may be due to integration errors in Gazebo, still these results are really bad to me. It seems to me strange that gravity compensation is so bad. I cannot understand why. Furthermore, this is what happens if I try to control only 1 joint (in the picture: torso pitch, joint torque):

I cannot understand this at all. Why I can't control only the torso pitch in torque mode?

This is what is inside the gravity compensation block:

Obtain geometry of the contact/support surface from the iCub skin

Until now, the geometry of the contact surface (typically the sole of the feet) has been hardcoded in the controllers using the phys.footSize and gain.footSize parameters [1], that express the support area as a rectangle in the l_sole or r_sole frames. This information is important
because it imposes the set of physical contact force/torques that the robot can exert on the contact surface without slipping or tipping.

For the CoDyCo Y4 Demo, we needed to balance the robot with the l_upper_leg/r_upper_leg links that are in contact with a side of the structural beam profiles, such as this one:

As the beam profile is in contact only with part of the cover of the *_upper_leg link, we need to estimated online with part of the leg is actually in contact with the beam profile. This can be done with the iCub skin, but need some custom software.

[1] See for example : https://github.com/robotology-playground/WBI-Toolbox-controllers/blob/e88439c24bcaecd3f2bfad4629cf359073a154ea/controllers/torqueBalancing/app/robots/iCubGenova02/gains.m#L122 .

Binary files committed to branch newControlArchitectureCleanedUp

Hi @pi-q,
I noticed that you pushed binary files to this repo, e.g. this.

This is a really bad practice since every pull now takes a long time.

Please pay extreme attention to this, and please fix the mess as soon as possible.

All balancing controllers fail to initialize

I updated yarp, icub-main and codyco-superbuild to the latest versions. Then, I run (not just compile) torqueBalancing.mdl. I got the following error:

[ERROR]yarpWholeBodyStates::init : estimator thread initialization failed 
[ERROR][mdlStart] Failed to initialize WBI

this occurs both in master and devel branch, and for all controllers (e.g. iCub, Walkman, ...) and also for this simple test:

this is what is going on in Matlab terminal:

[ERROR]yarpWholeBodySensors::readPwms failed for timeout
yarp: A thread failed to start with error code: 0
[ERR] Error while initializing yarpWholeBodyStates interface.

no other error messages are given. Tested on Ubuntu 16.04 LTS with Matlab 2016b, for now only in simulation.

Should we remove old configuration files?

Inside the app/robots folder of torqueBalancing and wholeBodyImpedance controllers there are configuration files for different robots. I think some of them are quite old and probably they are not even usable anymore. In particular:

torqueBalanging

iCubDarmstadt01
iCubGenova01
iCubGenova03
iCubGenova05
iCubParis01
iCubParis01

wholeBodyImpedance

iCubDarmstadt01

should we remove them? also considering that even if deleted we can recover them if necessary looking at github history.

cc @DanielePucci

twoFeetStandingIdleAndCalib.sh not working properly in simulation

Working with walkman in simulation I discover that the script twoFeetStandingIdleAndCalib.sh, that is a requirement for the robot simulation according with this README, was not working properly. In fact, the right foot contact wrench in the z direction apparently was -800 N. To solve this problem, I run instead the script yarp rpc /wholeBodyDynamics/rpc, and then resetOffset all. In this way the contact wrenches were measured correctly.

problem in running balancing controller

When I try to run the controller in balancing_with_opt_of_tau, MATLAB crashes and says that it needs to be closed!
Here is what I get in the terminal:

All parameters have been checked and passed correctly
Yarp read block will have: 23outputs
[INFO][mdlInitializeSize] BLOCK TYPE IS: 3
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 2
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 0
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 11
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 0
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 13
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 14
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 7
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 7
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 8
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 2
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 6
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

All parameters have been checked and passed correctly
Yarp read block will have: 23outputs
[INFO][mdlInitializeSize] BLOCK TYPE IS: 1
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 3
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 3
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 9
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 9
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 3
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlInitializeSize] BLOCK TYPE IS: 3
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini 
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][mdlInitializeSizes] ROBOT_DOF: 23 
[INFO][mdlInitializeSizes] Options set

[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 1 
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 0.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve joints angles
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 1 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot 
[INFO][robotStatus::robotConfig] Configuring...
||| clearing context
||| adding context [wholeBodyInterfaceToolbox]
||| configuring
||| no policy found
||| default config file specified as wholeBodyInterfaceToolbox.ini
||| checking [/Users/azadm/Documents/codyco-superbuild/main/WBIToolboxControllers/controllers/balancing_with_opt_of_tau/wholeBodyInterfaceToolbox.ini] (pwd)
||| checking [/Users/azadm/Library/Application Support/yarp/config/robots/icubGazeboSim] (robot YARP_CONFIG_HOME)
||| checking [/Users/azadm/Library/Application Support/yarp/robots/icubGazeboSim] (robot YARP_DATA_HOME)
||| checking [/Library/Preferences/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/etc/yarp/robots/icubGazeboSim] (robot YARP_CONFIG_DIRS)
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim] (robot YARP_DATA_DIRS)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/config/path.d] (robot path.d YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/wholeBodyInterfaceToolbox.ini] (robot)
||| checking [/Users/azadm/Library/Application Support/yarp/config/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_HOME)
||| checking [/Users/azadm/Library/Application Support/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_HOME)
||| checking [/Library/Preferences/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/etc/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_CONFIG_DIRS)
||| checking [/Users/azadm/Documents/yarp/build/share/yarp/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/icub-main/build/share/iCub/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox] (context YARP_DATA_DIRS)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini] (context)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/contexts/wholeBodyInterfaceToolbox/wholeBodyInterfaceToolbox.ini
||| finding file [yarpWholeBodyInterface.ini]
||| checking [/Users/azadm/Documents/codyco-superbuild/main/WBIToolboxControllers/controllers/balancing_with_opt_of_tau/yarpWholeBodyInterface.ini] (pwd)
||| checking [/Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini] (robot)
||| found /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][robotStatus::robotConfig] wbiConfFileFullPath is: /Users/azadm/Documents/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/yarpWholeBodyInterface.ini
[INFO][robotStatus::robotConfig] List of properties for yarpWholeBodyInterface was interpreted correctly
[INFO][robotStatus::robotConfig] robot_fixed is false!
[DEBUG][robotStatus::robotConfig] After reading from config file, params are 
[DEBUG][robotStatus::robotConfig] robot name:            icubGazeboSim 
[DEBUG][robotStatus::robotConfig] local name:            balancing 
[DEBUG][robotStatus::robotConfig] world reference frame: l_sole 
[DEBUG][robotStatus::robotConfig] WBI ID List:           ROBOT_TORQUE_CONTROL_JOINTS_WITHOUT_PRONOSUP 
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingactuator/torso/rpc:o active at tcp://147.188.200.113:10040
yarp: Port /balancingactuator/torso/command:o active at tcp://147.188.200.113:10041
yarp: Port /balancingactuator/torso/state:i active at tcp://147.188.200.113:10042
yarp: Port /balancingactuator/torso/stateExt:i active at tcp://147.188.200.113:10043
yarp: Sending output from /balancingactuator/torso/rpc:o to /icubGazeboSim/torso/rpc:i using tcp
yarp: Sending output from /balancingactuator/torso/command:o to /icubGazeboSim/torso/command:i using udp
yarp: Receiving input from /icubGazeboSim/torso/state:o to /balancingactuator/torso/state:i using udp
yarp: Receiving input from /icubGazeboSim/torso/stateExt:o to /balancingactuator/torso/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingactuator/left_arm/rpc:o active at tcp://147.188.200.113:10044
yarp: Port /balancingactuator/left_arm/command:o active at tcp://147.188.200.113:10045
yarp: Port /balancingactuator/left_arm/state:i active at tcp://147.188.200.113:10046
yarp: Port /balancingactuator/left_arm/stateExt:i active at tcp://147.188.200.113:10047
yarp: Sending output from /balancingactuator/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i using tcp
yarp: Sending output from /balancingactuator/left_arm/command:o to /icubGazeboSim/left_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/state:o to /balancingactuator/left_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/stateExt:o to /balancingactuator/left_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingactuator/right_arm/rpc:o active at tcp://147.188.200.113:10048
yarp: Port /balancingactuator/right_arm/command:o active at tcp://147.188.200.113:10049
yarp: Port /balancingactuator/right_arm/state:i active at tcp://147.188.200.113:10050
yarp: Port /balancingactuator/right_arm/stateExt:i active at tcp://147.188.200.113:10051
yarp: Sending output from /balancingactuator/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i using tcp
yarp: Sending output from /balancingactuator/right_arm/command:o to /icubGazeboSim/right_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/state:o to /balancingactuator/right_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/stateExt:o to /balancingactuator/right_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingactuator/left_leg/rpc:o active at tcp://147.188.200.113:10052
yarp: Port /balancingactuator/left_leg/command:o active at tcp://147.188.200.113:10053
yarp: Port /balancingactuator/left_leg/state:i active at tcp://147.188.200.113:10054
yarp: Port /balancingactuator/left_leg/stateExt:i active at tcp://147.188.200.113:10055
yarp: Sending output from /balancingactuator/left_leg/rpc:o to /icubGazeboSim/left_leg/rpc:i using tcp
yarp: Sending output from /balancingactuator/left_leg/command:o to /icubGazeboSim/left_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/state:o to /balancingactuator/left_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/stateExt:o to /balancingactuator/left_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingactuator/right_leg/rpc:o active at tcp://147.188.200.113:10056
yarp: Port /balancingactuator/right_leg/command:o active at tcp://147.188.200.113:10057
yarp: Port /balancingactuator/right_leg/state:i active at tcp://147.188.200.113:10058
yarp: Port /balancingactuator/right_leg/stateExt:i active at tcp://147.188.200.113:10059
yarp: Sending output from /balancingactuator/right_leg/rpc:o to /icubGazeboSim/right_leg/rpc:i using tcp
yarp: Sending output from /balancingactuator/right_leg/command:o to /icubGazeboSim/right_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/state:o to /balancingactuator/right_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/stateExt:o to /balancingactuator/right_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]Found world reference frame mention in yarpConfig. Setting as  l_sole 
[INFO]Reference link set as world was  9 
[INFO], now it is set to  9 
[INFO]yarpWholeBodyStates : readSpeedAccFromControlBoard option not found, reading velocities and accelerations from high level numerical derivatives 
[INFO]yarpWholeBodyStates : estimateBasePosAndVel option not found, we are estimating base position and velocity 
[WARN] yarpWholeBodyStates::loadCouplingsFromConfigurationFile : WBI_YARP_JOINTS_MOTOR_KINEMATIC_COUPLINGS group not found in configuration file
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingstate/torso/rpc:o active at tcp://147.188.200.113:10060
yarp: Port /balancingstate/torso/command:o active at tcp://147.188.200.113:10061
yarp: Port /balancingstate/torso/state:i active at tcp://147.188.200.113:10062
yarp: Port /balancingstate/torso/stateExt:i active at tcp://147.188.200.113:10063
yarp: Sending output from /balancingstate/torso/rpc:o to /icubGazeboSim/torso/rpc:i using tcp
yarp: Sending output from /balancingstate/torso/command:o to /icubGazeboSim/torso/command:i using udp
yarp: Receiving input from /icubGazeboSim/torso/state:o to /balancingstate/torso/state:i using udp
yarp: Receiving input from /icubGazeboSim/torso/stateExt:o to /balancingstate/torso/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingstate/left_arm/rpc:o active at tcp://147.188.200.113:10064
yarp: Port /balancingstate/left_arm/command:o active at tcp://147.188.200.113:10065
yarp: Port /balancingstate/left_arm/state:i active at tcp://147.188.200.113:10066
yarp: Port /balancingstate/left_arm/stateExt:i active at tcp://147.188.200.113:10067
yarp: Sending output from /balancingstate/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i using tcp
yarp: Sending output from /balancingstate/left_arm/command:o to /icubGazeboSim/left_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/state:o to /balancingstate/left_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/stateExt:o to /balancingstate/left_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingstate/right_arm/rpc:o active at tcp://147.188.200.113:10068
yarp: Port /balancingstate/right_arm/command:o active at tcp://147.188.200.113:10069
yarp: Port /balancingstate/right_arm/state:i active at tcp://147.188.200.113:10070
yarp: Port /balancingstate/right_arm/stateExt:i active at tcp://147.188.200.113:10071
yarp: Sending output from /balancingstate/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i using tcp
yarp: Sending output from /balancingstate/right_arm/command:o to /icubGazeboSim/right_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/state:o to /balancingstate/right_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/stateExt:o to /balancingstate/right_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingstate/left_leg/rpc:o active at tcp://147.188.200.113:10072
yarp: Port /balancingstate/left_leg/command:o active at tcp://147.188.200.113:10073
yarp: Port /balancingstate/left_leg/state:i active at tcp://147.188.200.113:10074
yarp: Port /balancingstate/left_leg/stateExt:i active at tcp://147.188.200.113:10075
yarp: Sending output from /balancingstate/left_leg/rpc:o to /icubGazeboSim/left_leg/rpc:i using tcp
yarp: Sending output from /balancingstate/left_leg/command:o to /icubGazeboSim/left_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/state:o to /balancingstate/left_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/stateExt:o to /balancingstate/left_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /balancingstate/right_leg/rpc:o active at tcp://147.188.200.113:10076
yarp: Port /balancingstate/right_leg/command:o active at tcp://147.188.200.113:10077
yarp: Port /balancingstate/right_leg/state:i active at tcp://147.188.200.113:10078
yarp: Port /balancingstate/right_leg/stateExt:i active at tcp://147.188.200.113:10079
yarp: Sending output from /balancingstate/right_leg/rpc:o to /icubGazeboSim/right_leg/rpc:i using tcp
yarp: Sending output from /balancingstate/right_leg/command:o to /icubGazeboSim/right_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/state:o to /balancingstate/right_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/stateExt:o to /balancingstate/right_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[DEBUG] yarpWholeBodyStates correctly initialized 
[INFO][robotStatus::robotConfig] Whole Body Interface correctly initialized
[INFO][robotStatus::robotConfig] About to set control mode
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /model/torso/rpc:o active at tcp://147.188.200.113:10080
yarp: Port /model/torso/command:o active at tcp://147.188.200.113:10081
yarp: Port /model/torso/state:i active at tcp://147.188.200.113:10082
yarp: Port /model/torso/stateExt:i active at tcp://147.188.200.113:10083
yarp: Sending output from /model/torso/rpc:o to /icubGazeboSim/torso/rpc:i using tcp
yarp: Sending output from /model/torso/command:o to /icubGazeboSim/torso/command:i using udp
yarp: Receiving input from /icubGazeboSim/torso/state:o to /model/torso/state:i using udp
yarp: Receiving input from /icubGazeboSim/torso/stateExt:o to /model/torso/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /model/left_arm/rpc:o active at tcp://147.188.200.113:10084
yarp: Port /model/left_arm/command:o active at tcp://147.188.200.113:10085
yarp: Port /model/left_arm/state:i active at tcp://147.188.200.113:10086
yarp: Port /model/left_arm/stateExt:i active at tcp://147.188.200.113:10087
yarp: Sending output from /model/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i using tcp
yarp: Sending output from /model/left_arm/command:o to /icubGazeboSim/left_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/state:o to /model/left_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_arm/stateExt:o to /model/left_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /model/right_arm/rpc:o active at tcp://147.188.200.113:10088
yarp: Port /model/right_arm/command:o active at tcp://147.188.200.113:10089
yarp: Port /model/right_arm/state:i active at tcp://147.188.200.113:10090
yarp: Port /model/right_arm/stateExt:i active at tcp://147.188.200.113:10091
yarp: Sending output from /model/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i using tcp
yarp: Sending output from /model/right_arm/command:o to /icubGazeboSim/right_arm/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/state:o to /model/right_arm/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_arm/stateExt:o to /model/right_arm/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /model/left_leg/rpc:o active at tcp://147.188.200.113:10092
yarp: Port /model/left_leg/command:o active at tcp://147.188.200.113:10093
yarp: Port /model/left_leg/state:i active at tcp://147.188.200.113:10094
yarp: Port /model/left_leg/stateExt:i active at tcp://147.188.200.113:10095
yarp: Sending output from /model/left_leg/rpc:o to /icubGazeboSim/left_leg/rpc:i using tcp
yarp: Sending output from /model/left_leg/command:o to /icubGazeboSim/left_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/state:o to /model/left_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/left_leg/stateExt:o to /model/left_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
RemoteControlBoard is ENABLING the writeStrict option for all commands
yarp: Port /model/right_leg/rpc:o active at tcp://147.188.200.113:10096
yarp: Port /model/right_leg/command:o active at tcp://147.188.200.113:10097
yarp: Port /model/right_leg/state:i active at tcp://147.188.200.113:10098
yarp: Port /model/right_leg/stateExt:i active at tcp://147.188.200.113:10099
yarp: Sending output from /model/right_leg/rpc:o to /icubGazeboSim/right_leg/rpc:i using tcp
yarp: Sending output from /model/right_leg/command:o to /icubGazeboSim/right_leg/command:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/state:o to /model/right_leg/state:i using udp
yarp: Receiving input from /icubGazeboSim/right_leg/stateExt:o to /model/right_leg/stateExt:i using udp
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
yarp: Removing output from /model/torso/rpc:o to /icubGazeboSim/torso/rpc:i
yarp: Removing output from /model/torso/command:o to /icubGazeboSim/torso/command:i
yarp: Removing input from /icubGazeboSim/torso/state:o to /model/torso/state:i
yarp: Removing input from /icubGazeboSim/torso/stateExt:o to /model/torso/stateExt:i
yarp: Removing output from /model/left_arm/rpc:o to /icubGazeboSim/left_arm/rpc:i
yarp: Removing output from /model/left_arm/command:o to /icubGazeboSim/left_arm/command:i
yarp: Removing input from /icubGazeboSim/left_arm/state:o to /model/left_arm/state:i
yarp: Removing input from /icubGazeboSim/left_arm/stateExt:o to /model/left_arm/stateExt:i
yarp: Removing output from /model/right_arm/rpc:o to /icubGazeboSim/right_arm/rpc:i
yarp: Removing output from /model/right_arm/command:o to /icubGazeboSim/right_arm/command:i
yarp: Removing input from /icubGazeboSim/right_arm/state:o to /model/right_arm/state:i
yarp: Removing input from /icubGazeboSim/right_arm/stateExt:o to /model/right_arm/stateExt:i
yarp: Removing output from /model/left_leg/rpc:o to /icubGazeboSim/left_leg/rpc:i
yarp: Removing output from /model/left_leg/command:o to /icubGazeboSim/left_leg/command:i
yarp: Removing input from /icubGazeboSim/left_leg/state:o to /model/left_leg/state:i
yarp: Removing input from /icubGazeboSim/left_leg/stateExt:o to /model/left_leg/stateExt:i
yarp: Removing output from /model/right_leg/rpc:o to /icubGazeboSim/right_leg/rpc:i
yarp: Removing output from /model/right_leg/command:o to /icubGazeboSim/right_leg/command:i
yarp: Removing input from /icubGazeboSim/right_leg/state:o to /model/right_leg/state:i
yarp: Removing input from /icubGazeboSim/right_leg/stateExt:o to /model/right_leg/stateExt:i
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED

[INFO][mdlStart] I AM GETTING UPDATED!!! 
YARP NETWORK INITIALIZED
YARP is running!!

yarp: Port /tmp/port/2 active at tcp://147.188.200.113:10101
[From] Port name will be: /y2coordinator/qdes:o
[To] Port name will be:   /tmp/port/2
Blocking? : 0
Timestamp? : 0
Autoconnect? : 1
Opening log file:  /Users/azadm/java.log.22968
Killed: 9

Is there any extra setting that needs to be done before running this file?

Remove branches "statemachine", "25dofs" and "fastMotionsLumix"

In particular, 25Dofs and fastMotionsLumix are 0 commit ahead master.

statemachine is 1 commit ahead master, but it just added 2 files. I can copy-paste these 2 files in legacyfolder.

cc owners of these branches : @francesco-romano @pi-q @DanielePucci .

[BUG] README with broken links

The README of the project has a broken link in the torqueBalancingJc README.

[WBI-Toolbox-controllers] Migration of all balancing controllers into one called "torqueBalancing"

I've attempted at unifying all balancing controllers developed so far into only one simulink model. This new controller is under the folder "torqueBalacing". Beaware that this new controller depends on the solver QPOASES, which must be installed accordingly, e.g. in a terminal type

cd $CODYCO_SUPERBUILD_ROOT
git pull
cd build
cmake .
make update-all
ccmake .

Then set CODYCO_BUILD_QPOASES = ON, press "c" until "g" appears, then press "g", and type

make

Tell me if you have troubles with this new version of the controller in simulations @rlober @azadm @francesco-romano @jeljaik @nunoguedelha @naveenoid

Configuration files for torqueBalancing

For the time being, support of configuration files is maintained only for icubGazeboSim, iCubDarmstadt01, and iCubGenova01.

Gravity compensation not working properly on torso

We tried the wholeBodyImpedance controller by zeroing the gains. We were expecting that the gravity compensation would more or less do a good job, being the model reliable right now. In fact,, this was the case for the legs and the arms, but the torso was still falling quite abruptly. @traversaro @francesco-romano @nunoguedelha

Port some branches in master

I'm referring to these branches:

I think I can port all these controllers in master. In particular:

seesaw has its own folder in master, and won't affect the main balancing controller;
walkman just add the config files for walkman. We can move the model for calibration in the walkman-element;
icub_standUp add a new state machine in the model, but it shouldn't affect the state machine for yoga.

I think it is probably easier to port the part of the code related to these demos in master instead of merging the branches.

"Yoga" demo tutorial

For education purpose it would be nice to have a tutorial explaining how to run the so-called "Yoga" demo in simulation.

cc @DanielePucci @francesco-romano @fjandrad

Move repository to robotology

As discussed with @traversaro, this repository should be moved to robotology

cc-issue: robotology/robotology.github.io#2

Avoid .slx binary files in this repository

@pi-q recently uploaded a new version of a fixed-base manipulator controller we are working on. However, the model has an .slx extension, which should be avoided as much as possible.

Simulink compilation stops at "Compiling: evaluating block parameters: Started"

See robotology/wb-toolbox#40

New procedure for iCub stand up demo

In november-december 2016 most of the software that is used for running balancing simulations (yarp, yarp-wholebodyinterface, icub-gazebo) has been updated. In particular, the original procedure for running the iCub stand up demo was the following:

Switch to demo_codyco branch in yarp-wholebodyinterface repo;
Switch to icub_chair branch in icub-gazebo repo;
Switch to demo_codyco branch in WBIToolbox-controllers repo;
Run gazebo_chair from a terminal. This command is an alias to automatically load the world with iCub and the chair in gazebo.

After the general update in november-december, this procedure has been changed. In particular:

In yarp-wholebodyInterface, the branch demo_codyco has been merged in master so it is no more necessary to switch branches;
The world with iCub and the chair in icub-gazebo moved to a new repository, icub-gazebo-wholebody. The new repo can be installed and used exactly as icub-gazebo, just append its path to your .bashrc;
The branch demo_codyco in WBIToolbox-controllers has been updated and renamed as icub_chair.

I apologize if this update was not clearly documented before, I thought that for the moment it was enough to update the CODYCO_DEMO_README in icub_chair branch.

EDIT
iCub stand up demo is working with icubGazeboSim and iCubGenova02

Problems with Clock when following the torqueBalancing instructions

I am getting the following error

Error reported by S-function 'ySynchronizer' in 'torqueBalancing/Subsystem/synchronizer/synchronize':
Problems connecting with /clock/rpc. Has gazebo been launched with the clock plugin?

which can be solved by removing or commenting out the blocks for the synchronisation. However, none of the models in repository https://github.com/robotology-playground/icub-gazebo seems to use the clock plugin.

torqueBalancing on right foot

Hi,
We usually perform the torque balancing on single leg with the left leg.
But I wanted to try the torqueBalancing with the robot standing on the right foot.
I did the same procedures I usually do with the left leg (launching also wholeBodyDynamicsTree with --assume_fixed r_foot_dh_frame and calibrate with calibStandingRightFoot), but this does not work.
When I launch the Simulink the robot fell, i.e. the left leg fell.

Should I adjust something else?
Thanks!
@DanielePucci

Move completeForceControlScripts in legacy

I was wondering what is completeForceControlScripts folder used for. It only contains two matlab scripts (last update 2015). Can I move it inside legacy folder?

cc: @DanielePucci

Update documentation of the repo

It seems to me the README.md files in every controller folder are outdated. An example:

in this README, it is stated that one must run wholeBodyDynamicsTree in simulation, which has been substituted by wholeBodyDynamics. Also twoFeetStandingIdleAndCalib is not used anymore in simulation, and it is substituted by sending resetOffset all command to wholeBodyDynamics.

wholeBodyImpedance need to be ported to the WBT 2.0

@gabrielenava @DanielePucci

question about the floating base calculations

I am wondering what the first 16 elements of q of the floating base are and why they are set to 0 for calculation of the equilibrium point?
Thanks in advance

icubGazeboSim and iCubGenova02 models have frames oriented differenlty

There is a mismatch between the iCub model used for simulations and the one of iCubGenova02. The frames l_upper_leg contact and r_upper_leg_contact are oriented differently, as it is possible to see in the following images:

these frames are currently used only for iCub stand up simulation. Since l_upper_leg_contact at the initial time is taken as inertial reference frame, this mismatch causes the CoM references to have different signs in simulations and on the robot. The "correct" orientation is the one of icubSim, because it is the same of l_sole (while sitting) and therefore in this way all demos (Yoga, Coordinator, Standup) have the same references signs. Since I am for now the only user of iCub stand up demo on iCubGenova02, I will take care of regenerating the frames from CAD.

Cleanup of directory names

The mantained version of the controllers that use WB-Toolbox 2.0 should have obvious names, not the current torqueBalancing_WBT. Otherwise it is certaint that almost all user would open the old one by mistake.

I propose to move the "old" controllers in a legacy subfolder:

controllers/torqueBalancing --> controllers/legacy/torqueBalancing
controllers/wholeBodyImpedance --> controllers/legacy/wholeBodyImpedance

Then we can rename the current versions to have intuitive names:

controllers/torqueBalancingWBT --> controllers/torqueBalancing

cc @DanielePucci @gabrielenava
ref robotology/wb-toolbox#10

Simulink-Gazebo crash combos

Working with Gazebo simulator and Simulink controller I found some bugs related to either Gazebo, or Simulink, or other programs used for the simulation (wholeBodyDynamics3, yarpmotorgui, ...). I decided to list here all the "combos" that make Gazebo/Simulink/other stuff crash.

If I delete the model from gazebo while wholeBodyDynamics3 is still running, and I try to insert the model again, the new robot disappear from gazebo. If I stop the wholeBodyDynamics3, I can insert the new model without problems. There are no error or warnings related to this issue.
Remove the robot (with yarpmotorgui running), then select a new one without inserting it in the Gazebo world. Then, if you close the yarpmotorgui, Gazebo crashes.
If you remove the robot from Gazebo before stopping the simulation, Simulink crashes (this is more a warning than an issue)

Please update this issue if you find a new combo.