search

jeljaik / WB-Team-Issues

This repository serves as a container of all the main issues related to the realization of the first year demo of the european project KOROIBOT.
0 stars 0 forks source link

Simulink Synchronization #26

Open jeljaik opened 9 years ago

jeljaik commented 9 years ago

The block ySynchronizer in the WBI-Toolbox is in charge of controlling the simulation time of Gazebo given a desired simulation period.

francesco-romano commented 9 years ago

Reference: http://it.mathworks.com/help/simulink/ug/using-the-set-param-command.html

francesco-romano commented 9 years ago
jeljaik commented 9 years ago

@nunoguedelha @prashanthr05 As we discussed earlier, before proceeding with better (?) tuning of the ODE friction model used in Gazebo, let us first toroughly test the ySynchronizer. This is the original issue in which along with Francesco Romani we started our work on this topic. Some results were achieved and some other issues appeared. I suggest the following readings:

  1. https://github.com/robotology-playground/WBI-Toolbox/wiki/YARP-Synchronization
  2. http://gazebosim.org/tutorials/?tut=plugins_hello_world

Let us keep track of our progress in this issue and nowhere else :wink:

jeljaik commented 9 years ago

Related Issues

jeljaik commented 9 years ago

Currently the biggest issue to investigate is the following:

When launching any controller containing the ySynchronizer block, the configuration of yarpWholeBodyInterface fails, in particular, that of the wholeBodyStates due to the reacing of a timeout on the encoders: 

[ERROR]yarpWholeBodySensors::readEncoders failed for timeout
yarp: A thread failed to start with error code: 0
[ERR] yarpWholeBodyStates::init : estimator thread initialization failed

This should not happen if gazebo is being launched with the clock plugin and the environmental variable YARP_CLOCK is set. Still it happens, as if the time seen by wholeBodyStates was machine time, instead of the one streamed by Gazebo!

@nunoguedelha @prashanthr05

jeljaik commented 9 years ago

Update

I have investigated a bit deeper the issue just mentioned above and did the following:

  1. Tweaked the yarpWholeBodyInterfaceTest module to include an object of type ClockServer doing the same things that were done during initialization of the ySynchronizer thus finding out that the option resetSimulationTime is troublesome! When this rpc command is issued the encoders timeout is fired, thus aborting the interface configuration.

Of course, after solving the previous issue a bigger one showed up by which Matlab gets stuck during configuration with the following log:

[INFO][mdlInitializeSize] BLOCK TYPE IS: 3
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: 1
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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
All parameters have been checked and passed correctly
All parameters have been checked and passed correctly
[INFO][mdlInitializeSize] BLOCK TYPE IS: 1
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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
[DEBUG][mdlInitializeSizes] Options set for ySynchronizer
[INFO]YARP Network initialized by ySynchronizer
[INFO][ySynchronizer::mdlStart] >> clockServer client retrieved stepSize = 0.001000
[INFO]ySynchronizer::mdlStart >> Finished startup
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 28
[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...
[INFO][robotStatus::robotConfig] wbiConfFileFullPath is: /home/jorhabib/Software/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
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[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] yarpWholeBodySensors: initializing with 23 encoders 23 pwm sensors 0 F/T sensors 0 IMUs
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[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
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.

Log from a simulation that randomly worked with the ySynchronizer block:

[INFO][mdlInitializeSize] BLOCK TYPE IS: 3
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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: 1
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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
All parameters have been checked and passed correctly
All parameters have been checked and passed correctly
[INFO][mdlInitializeSize] BLOCK TYPE IS: 1
[INFO][mdlInitializeSizes] wbiConfFileName yarpWholeBodyInterface.ini
[INFO][mdlInitializeSizes] wbiConfFileFullPath is: /home/jorhabib/Software/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: /home/jorhabib/Software/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: /home/jorhabib/Software/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
[DEBUG][mdlInitializeSizes] Options set for ySynchronizer
[INFO]YARP Network initialized by ySynchronizer
[INFO][ySynchronizer::mdlStart] >> clockServer client retrieved stepSize = 0.001000
[INFO]ySynchronizer::mdlStart >> Finished startup
[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...
[INFO][robotStatus::robotConfig] wbiConfFileFullPath is: /home/jorhabib/Software/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
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[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] yarpWholeBodySensors: initializing with 23 encoders 23 pwm sensors 0 F/T sensors 0 IMUs
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[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
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO]created device <remote_controlboard>. See C++ class yarp::dev::RemoteControlBoard for documentation.
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 2
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 8.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will compute mass matrix from dynamics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 2 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 3
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 3 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 1
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 4
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 4 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 0
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 5
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 1.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve joints velocities
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 5 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 6
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 7.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will compute generalized bias forces from dynamics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 6 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 7
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 11.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve joint torques
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 7 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 8
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 8 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 1
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 9
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 9 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 0
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 10
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 2.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric forward kinematics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 10 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 2
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
YARP is running!!
BLOCK TASK DOF PARAMETER:  4
SIMULATION RATE PARAMETER: 0.01
TRAJECTORY TIME PARAMETER: 3
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 11
[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 11 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[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 is running!!
BLOCK TASK DOF PARAMETER:  23
SIMULATION RATE PARAMETER: 0.01
TRAJECTORY TIME PARAMETER: 5
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 12
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 12 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 1
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 13
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 13 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 0
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 14
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 1.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve joints velocities
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 14 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 15
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 8.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will compute mass matrix from dynamics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 15 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 16
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 7.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will compute generalized bias forces from dynamics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 16 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 17
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 7.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will compute generalized bias forces from dynamics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 17 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 18
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 14.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve the centroidal momentum
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 18 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 19
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 2.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric forward kinematics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 19 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 0
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 20
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 20 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 1
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 21
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 21 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 0
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 22
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 9.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will compute dJdq
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 22 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 23
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 9.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will compute dJdq
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 23 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 24
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 2.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric forward kinematics
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 24 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 2
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 25
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 3.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will retrieve parametric Jacobians
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 25 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] READ LINK IS: 2
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
YARP is running!!
BLOCK TASK DOF PARAMETER:  3
SIMULATION RATE PARAMETER: 0.01
TRAJECTORY TIME PARAMETER: 20
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 26
[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 26 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlStart] STARTED
[INFO][mdlStart] Publicly stating that a new child has been born: 27
[INFO][mdlStart] The string being passed for robotName is - icubGazeboSim
[INFO][mdlStart] Block type mask parameter: 6.000000
[INFO][mdlStart] The link passed for parametric blocks is: na
[INFO][mdlOutputs] This block will set torques
[INFO][mdlStart] YARP is up and running!!
[INFO][robotStatus::robotStatus] robotStatus class instantiated 27 times
[INFO][mdlStart] An object robot of type wholeBodyInterface has been created
[INFO][mdlStart] About to configure robot
[INFO][robotStatus::robotConfig] Configuring...
[INFO][mdlStart] Succesfully exited robotConfig.
[INFO][robotStatus::robotInit] Finished robotInit
[INFO][mdlStart] Succesfully exited robotInit.
[INFO][mdlStart] FINISHED
[INFO][mdlStart] I AM GETTING UPDATED!!!
[INFO][mdlTerminate][ySynchronizer] Before issuing continueSimulation command
[INFO][mdlTerminate][ySynchronizer] reached end of this simulation
[INFO][mdlTerminate] Inside robot object 0x7f28866cd410
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286dbe8430
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286db81be0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286dbe8bf0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286db836a0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f51b4a0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f507300
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f446620
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f446d30
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f459f80
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f45f5b0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f463820
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f466330
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f468f80
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f46c9d0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f470c30
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f471340
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f474d10
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f478210
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f47d6f0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f47e440
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f481870
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f484a60
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f4882b0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f48dc70
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286f5a63a0
[INFO][mdlTerminate] Left mdlTerminate
[INFO][mdlTerminate] Inside robot object 0x7f286ec755e0
[INFO][mdlTerminate] ctrl mode set
[INFO][robotStatus::~robotStatus] wbInterface in destructor: 0x7f2887fa7050
[INFO][robotStatus::~robotStatus] wbInterface has been closed and deleted correctly. -1 to go
[INFO][mdlTerminate] robot deleted
[INFO][mdlTerminate] yarpWbiOptions deleted
[INFO][mdlTerminate] all variables set to NULL
[INFO][mdlTerminate] Left mdlTerminate