How to setup the enviroment for running torque control on iCub #10

gabrielenava · 2018-01-19T14:07:07Z

This issue is for collecting all the links to the documentation which is necessary to follow in order to run torque control simulations with iCub.

Basic requirements:

you need a PC with ubuntu (better if 16.04 LTS) or macOS. These are the operating systems we use in our lab, and they are tested and supported. Regarding Windows, the problem is that the Gazebo simulator is not supported for now.
Gazebo simulator. It is the software we use for running torque control simulations. Tutorial for installing Gazebo: http://gazebosim.org/tutorials?cat=install (run also the command for developers working on top of Gazebo: sudo apt-get install libgazebo8-dev (it is explained in the page).
you need a MATLAB/Simulink version greater than R2014a.

Installing yarp, icub-main and codyco-superbuild

Case 1: install yarp and icub-main outside the codyco-superbuild

The documentation on how to install yarp and icub-main can be found online, inside the iCub wiki. In google, just look for something like "how to install yarp and icub on linux from sources" (or on Mac if you have a mac). A wiki page like this will be among the first results: http://wiki.icub.org/wiki/Linux:Installation_from_sources. I personally suggest to download the source code of yarp from github, instead of installing it from binaries. Both solutions are explained here: http://www.yarp.it/install_yarp_linux.html. Furthermore, I strongly suggest NOT to use sudo make install, but instead to choose the other alternative (create a local folder named "install", and adjust the path inside your .bashrc file. Both solutions are explained in the wiki.

Case 2: install yarp and icub-main inside the codyco-superbuild

You still need to go in the wiki page, but just follow the instructions on how to get all the dependencies (the first section of the page). Do not dowload yarp and icub-main: in this case, we're going to install them later.

Install the codyco-superbuild

What is the codyco-superbuild?

As stated in its README,

This is a meta repository (so-called "superbuild") that uses CMake and YCM to automatically download and compile CoDyCo software. CMake is an open-source, cross-platform family of tools designed to build, test and package software. A YCM Superbuild is a CMake project whose only goal is to download and build several other projects.

In short, it is a way to easily download and compile all the required software for a specific project, in particular the CoDyCo project.

What should I do to install it?

Follow the instructions in the README. Important! If you did not already installed yarp and icub-main, they will be automatically installed inside the codyco-superbuild! If instead you already installed them, and you correctly set the path inside your .bashrc as explained here, last section, they won't be installed inside the superbuild.

Download the whole body torque control repository and its dependencies

This is the most complicated part. At the end of the codyco-superbuild README, it is explained how to install the matlab-dependent libraries needed for torque control, and it is also pointing to the WBIToolbox-controllers repository, which contains our "old" versions of Simulink balancing controllers.

We are currently releasing a new library for getting dynamic and kinematic quantities for torque control in Simulink and we're also releasing a new repository for storing the Simulink controllers, what is the one in which I'm writing this issue: wholeBodyControllers.

Given that the old repositories will be moved in legacy soon. I suggest to install the new ones, even if they are still under development.

To do so, follow the instructions in this README. Regarding the controller dependencise if you follow the instructions above you should have already installed most of them. What remains can be installed by setting the options CODYCO_USES_GAZEBO = ON and CODYCO_USES_MATLAB = ON before compiling codyco-superbuild.

Remark 1

If after typing ccmake ../ inside the CODYCO_SUPERBUILD/build directory you cannot find one of the two options, press t. More options will appear.

Remark 2

Check the icub-gazebo and icub-gazebo-wholebody readmes to see how to add the required iCub model to the Gazebo path.

controllers documentation: https://github.com/robotology-playground/wholeBodyControllers/tree/master/doc
documentation on how to run a simulation: https://github.com/robotology-playground/wholeBodyControllers/blob/master/doc/How-to-run-torqueBalancing-simulations-with-iCub.md
each controller has its own readme. Check it.
if you have a Matlab version lower than R2016b, but greater than R2014a, you can still open the models inside the legacy folder. For now it is not easy to deal with different Simulink versions, but we're working on it.

Things are still not working: where should I open issues?

If something it is not working (most common issue: something is not compiling when running the make command, or there are missing paths) check the name of the repository that is causing issues (e.g., yarp) and open an issue in that repository. The responsible will answer you.

If there are passages of this guide that are not clear, leave a comment in this issue.

The text was updated successfully, but these errors were encountered:

caicaixia · 2018-01-24T05:55:53Z

Hello, I tried to run the wholeBodyControllers in simulation as https://github.com/robotology-playground/wholeBodyControllers/blob/master/doc/How-to-run-torqueBalancing-simulations-with-iCub.md, but it's not successful.
But I have some questions:

after run $ gazebo -slibgazebo_yarp_clock.so, which robot model should I insert in the gazebo?
There several repos: icub-gazebo-wholebody/models has iCub fixed no hand, iCub feet fixed and so on; iCub/models has iCubGenova04, iCunGenova02, iCubGazeboV2_5 etc . More repos like gazebo/models, icub-gazebo.
I can't run the Simulink model, it shows some errors. Here is my steps:

export YARP_ROBOT_NAME=icubGazeboSim
$ yarpserver
$ gazebo -slibgazebo_yarp_clock.so, then insert an iCub model (iCub fixed no hands)
$ yarpmotorgui --from homePoseBalancing.ini -> Global Joints Commands/Custom postions/TwoFeetStanding
This step shows some errors like:

  [ERROR]Error while getting current range, part  head  joint  0 
  ...
  [ERROR]Error while getting current range, part  right_arm  joint  0 
  [ERROR]Error while getting current range, part  right_arm  joint  1....

$ YARP_ROBOT_NAME=icubGazeboSim yarprobotinterface --config launch-wholebodydynamics.xml
$ yarp rpc /wholeBodyDynamics/rpc ---> resetOffset all 300
This shows a warning: [WARNING]wholeBodyDynamics : calib ignoring calib_code all
Open the Simulink model and run the module (I use matlab2017b)

The file containing block diagram 'WBCLibrary' has been changed on disk since it was loaded.  You should close it, and Simulink will reload it if necessary.
Component:Simulink | Category:Block diagram warning
Error while obtaining sizes from MEX S-function 'WBToolbox' in 'impedanceControl/Get Estimate/S-Function'.
Caused by:
Invalid MEX-file '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64': 
Missing symbol '_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE7compareEPKc' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE10_M_replaceEmmPKcm' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE4swapERS4_' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE6assignEPKc' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE7reserveEm' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE9_M_appendEPKcm' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE9_M_assignERKS4_' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE9_M_createERmm' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEED1Ev' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNSt7__cxx1119basic_ostringstreamIcSt11char_traitsIcESaIcEED1Ev' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZTTNSt7__cxx1119basic_ostringstreamIcSt11char_traitsIcESaIcEEE' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZTVNSt7__cxx1115basic_stringbufIcSt11char_traitsIcESaIcEEE' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZTVNSt7__cxx1119basic_ostringstreamIcSt11char_traitsIcESaIcEEE' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
Missing symbol '_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE7compareEPKc' required by '/home/cai/Workspace/codyco-superbuild/build/install/lib/libyarpwholebodyinterface.so.0.3.6->/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64'
...

diegoferigo · 2018-01-24T09:03:38Z

Hi @caicaixia, waiting @gabrielenava for a more thorough reply on the other problems, can you please post the output of nm -C -u /path/to/WBToolbox.mexa64 which can be useful to debug the Invalid MEX-file error? It looks like some symbols are unresolved.

You can find the file in the <INSTALL_PREFIX>/mex folder.

It turned out being a problem of Matlab's dynamic libraries linking. Here a more detailed explanation.

caicaixia · 2018-01-24T09:54:44Z

I solve the second error about gcc version, Missing symbol '_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE7compareEPKc' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64' , by add this line to .bashrc file:
export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libprotobuf.so.9
Somebody said it is the gcc version problem for matlab.

However, the Simulink still cannot run successfully.

I set the setenv('YARP_ROBOT_NAME','icubGazeboSim'); in the init**.m files.
Then run the .mdl. Should I run the initTorqueBalancingYoga.m first, or the .mdl will directly call it?
This shows some errors in Matlab:
When I run the demo torqueBalancingYoga, it shows the same errors in the matlab also, and the robot in the gazebo is not moving.

gabrielenava · 2018-01-24T10:17:02Z

Hi @caicaixia, thanks for the feedback! Ok so, regading this:

after run $gazebo -slibgazebo_yarp_clock.so, which robot model should I insert in the gazebo?
There several repos: icub-gazebo-wholebody/models has iCub fixed no hand, iCub feet fixed and so on; iCub/models has iCubGenova04, iCunGenova02, iCubGazeboV2_5 etc . More repos like gazebo/models, icub-gazebo.

iCubGenova04 and iCubGenova02 are the names of the iCub we have in our lab, in Genova. Their models are stored in icub-models, but they are not configured to be used in Gazebo, they are used only to control the real robots (if you try to insert these models in Gazebo, you'll probably get an error).
iCubGazeboV2_5 is a new model, also stored into the icub-models. It is working, but still under development. I suggest not to use it for now, also because the procedure to have it working properly may imply to use developers branches in other repos (e.g. icub-gazebo-wholebody).
So, you correctly configured the robot name: the one to be used for simulations is icubGazeboSim. There are different models available for different controllers:

impedanceControl: the correct model is icub fixed (no hands), stored into icub-gazebo-wholebody. No particular home position is required.

torqueBalancingYoga: the correct model is icub (no hands), stored into icub-gazebo. The corresponding home position is homePoseYogaPP.

torqueBalancingStandup: the model is always icub (no hands), but it is complicated to sit down the robot. For this reason, a dedicated world in Gazebo has been created. Documentation is also here: no home position is required (the robot is already in its home position).

torqueWalkingMPC: the model is icub (no hands). The EXAMPLE_STATE_MACHINE demo is already available, I usually use as home position the homePoseYogaPP. Beware of this issue related to the blocking read on the ports (remove the check on the blocking read on the qt_fixed_LFoot block). Regarding the WALKING demo, it requires other repo that are is still private, so it is impossible to use it for now.

NOTE: also the models with hands can be used, but they are unnecessarily heavy (unless you want to use the hands for grasping an object).

Related instead to these errors:

  [ERROR]Error while getting current range, part  head  joint  0 
  ...
  [ERROR]Error while getting current range, part  right_arm  joint  0 
  [ERROR]Error while getting current range, part  right_arm  joint  1....

[WARNING]wholeBodyDynamics : calib ignoring calib_code all

they are documented in other issues, and someone is taking care of them, but they do not affect the simulations at all. The same goes for this warning (you will see it when the controller is running). Another possible error: CHECK NUMBER OF ENTRIES: HEAD while homing the robot. I'm on this, but it does not affect the home position. If you click ok, it will home the robot anyway.

Regarding the last issue, I remember I opened this issue long time ago: robotology-legacy/codyco-superbuild#141

In particular, follow the WBToolbox readme:

I think that if you do what is suggested in the troubleshooting you don't need anymore the export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libprotobuf.so.9 that you did.

Try with this configuration, and tell us if there are still issues :-).

gabrielenava · 2018-01-24T10:30:31Z

Update:

Should I run the initTorqueBalancingYoga.m first, or the .mdl will directly call it?

The .mdl will directly call it.

caicaixia · 2018-01-25T05:55:04Z

I tried all the demos, it's still not working.

Matlab issue: I did the one as troubleshooting, and tried both ways like
a) LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.19 matlab;
b) The permanent and scalable solution in this issue. However, both of them have the same issue.
Missing symbol '_ZNKSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEE7compareEPKc' required by '/home/cai/Workspace/codyco-superbuild/build/install/mex/WBToolbox.mexa64.
I have to do it as export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6:/usr/lib/x86_64-linux-gnu/libprotobuf.so.9
demo torqueBalancingYoga: it shows many errors in matlab, and the robot in the gazebo is not moving. (terminating)
demo torqueBalancingStandup: I set up the dedicated world, and I can see a iCub sitting on a chair.

source /usr/share/gazebo/setup.sh
export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/home/cai/Workspace/codyco-superbuild/build/install/share/gazebo/worlds/icub_standup_world

$ gazebo -slibgazebo_yarp_clock.so icub_standup_world

However, after I run the Simulink .mdl, it has the same error as the previous demo and the iCub doesn't move.

demo impedanceControl: the matlab also has the same errors, but the robot is moving in gazebo. However, I am not sure if the iCub moves correctly. At least the matlab was running.

Notes: I run many times, every time the motion of the robots are different. The torque plots are also different.

Remarks:

In all demos, I changed the setenv('YARP_ROBOT_NAME','icubGazeboSim'); in the init**.m
I used matlab 2017b.

diegoferigo · 2018-01-25T07:38:07Z

@caicaixia In the next weeks we will release a new version of the WB-Toolbox which will require only minor modifications from the user side. I improved the toolbox documentation (you can reach it temporarily in my personal fork) including a more stable fix to the LD_PRELOAD workaround.

As you can see, the library is addressed only by its major release (6), which is a symlink to the minor (6.0.22 in my setup). The latter is dependent on the distribution you're using. I'm pretty sure that your system has no 6.0.19 version, the one reported in the README, which should be updated.

Some links for cross referencing this issues with other ones: robotology-legacy/codyco-superbuild#141 (comment).

It is still not clear to me the need of adding in the preload path also libprotobuf.

caicaixia · 2018-01-25T08:36:23Z

yes, I already checked it my computer, my system only have /usr/lib/x86_64-linux-gnu/libstdc++.so.6 and libstdc++.so.6.0.21

Thats why I tried run like $ LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab in the terminal, but its not working. I also tried to set the LDPATH_PREFIX='/usr/local/opt/gcc/lib/gcc/6/' in the .matlab7rc.sh, but it is also not working.

However, if I remove libprotobuf, only put this line in .bashrc, it doesn't show the errors.
export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6

I don't know why :(

After removing libprotobuf , I tested the demo torqueBalancingYoga again. The error is different now, but the simulik still was terminated.

Is this the problem of my matlab version (Matlab 2017b)? What should I do because the .mdl models is saved by Matlab2016b ?

gabrielenava · 2018-01-25T11:03:05Z

mmh, this is really strange. I tried to replicate the issue on my laptop.

I have a personal laptop with Ubuntu 16.04 LTS and Matlab R2017b. I removed all the installed folders (I have everything installed locally) and I reinstalled everything from scratch. I chose the option to install YARP and ICUB-MAIN inside the CODYCO-SUPERBUILD. Usually, we have some repositories in devel branch instead of master branch, but for this test I tried to run the controllers with everything in master branch. I run the Yoga controller and I didn't get any error, and the controller was able to run without issues.

I'm worried this issue is related to something missing/different in your PC, and this is way more difficult to debug. Maybe @traversaro can help us debugging this.

caicaixia · 2018-01-26T02:23:53Z

I installed the YARP and ICUB-MAIN from the source codes first, like http://wiki.icub.org/wiki/Linux:Installation_from_sources.

Then I install the gazebo8 and gazebo-yarp-plugins. Finally I did CODYCO-SUPERBUILD.

When I install the CODYCO-SUPERBUILD, there are so many flags, which flags did you set to TRUE? In my ccmake ...:

 CODYCO_BUILD_QPOASES     ON

 CODYCO_TRAVIS_CI                 ON                                                                                                                                                                       
 CODYCO_USES_GAZEBO               ON                                                                                                                                                                       
 CODYCO_USES_KDL                  ON  

 CODYCO_USES_MATLAB               ON                                                                                                                                                                       

 CODYCO_USES_MEX_WHOLEBODYMODEL   ON 

 CODYCO_USES_WBI_TOOLBOX          ON                                                                                                                                                                       
 CODYCO_USES_WBI_TOOLBOX_CONTRO   ON  

 USE_SYSTEM_GAZEBOYARPPLUGINS     ON                                                                                                                                                                       
 USE_SYSTEM_ICUB                  ON                                                                                                                                                                       
 USE_SYSTEM_OROCOS_KDL            ON                                                                                                                                                                       
 USE_SYSTEM_TINYXML               ON                                                                                                                                                                       
 USE_SYSTEM_URDFDOM               ON                                                                                                                                                                       
 USE_SYSTEM_URDFDOM_HEADERS       ON                                                                                                                                                                       
 USE_SYSTEM_YARP                  ON

Do I miss some flags?

When we install packages, make install to /user/local/bin will cause some problems? Do we need to install all the things in local like /home/user/?
This is my local bashrc for icub:

#BASHRC_ICUB:

echo "Exporting iCub environment variables from specific file.."

# MATLAB
#----------------------------------------
#export PATH=$PATH:/home/icub/Documents/MATLAB/bin
alias matlab='cd ~/Documents/MATLAB && /usr/local/bin/matlab'
export LD_PRELOAD=$LD_PRELOAD:/usr/lib/x86_64-linux-gnu/libstdc++.so.6

#### yarp & icub related variables ###
export code=/home/cai/Workspace/codyco-superbuild/main

#GAZEBO VARIABLES
#----------------------------------------
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:/usr/local/lib/

if [ -z "$GAZEBO_MODEL_PATH" ]; then
    export GAZEBO_MODEL_PATH=$code/icub-gazebo:$code/icub-gazebo-wholebody/models:/home/cai/Workspace/codyco-superbuild/build/install/share/gazebo/models:/home/cai/share/iCub/robots:/home/cai/share
else
    export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:$code/icub-gazebo:$code/icub-gazebo-wholebody/models:/home/cai/Workspace/codyco-superbuild/build/install/share/gazebo/models:/home/cai/share/iCub/robots:/home/cai/share
fi

if [ -z "$GAZEBO_RESOURCE_PATH" ]; then
    export GAZEBO_RESOURCE_PATH=/home/cai/Workspace/codyco-superbuild/build/install/share/gazebo/worlds/icub_standup_world
else
    export GAZEBO_RESOURCE_PATH=$GAZEBO_RESOURCE_PATH:/home/cai/Workspace/codyco-superbuild/build/install/share/gazebo/worlds/icub_standup_world
fi

#YARP
#----------------------------------------

#choose your icub robot by commenting the robot that you do not use
#by default you can use iCub in gazebo
export YARP_ROBOT_NAME=icubGazeboSim  #to be used with Gazebo and codyco in simulink
#export YARP_ROBOT_NAME=icubSim
#export YARP_ROBOT_NAME=iCubNancy01     #our real iCub

export YARP_ROOT=/home/cai/Workspace/yarp
export YARP_DIR=$YARP_ROOT/build

#ICUB
#----------------------------------------
export ICUB_ROOT=/home/cai/Workspace/icub-main
export ICUB_DIR=$ICUB_ROOT/build
export PATH=$PATH:$YARP_DIR/bin:$ICUB_DIR/bin


#ICUB-CONTRIB-COMMON
#----------------------------------------
export ICUBcontrib_DIR=/home/cai/Workspace/icub-contrib-common/build
export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$ICUBcontrib_DIR


#CODYCO
#----------------------------------------
export CODYCO_SUPERBUILD_ROOT=/home/cai/Workspace/codyco-superbuild
export CODYCO_SUPERBUILD_DIR=$CODYCO_SUPERBUILD_ROOT/build

export PATH=$PATH:$CODYCO_SUPERBUILD_ROOT/build/install/bin
export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$CODYCO_SUPERBUILD_ROOT/build/install/lib


# YARP_DATA_DIRS
#----------------------------------------
export YARP_DATA_DIRS=$YARP_DATA_DIRS:$CODYCO_SUPERBUILD_ROOT/build/install/share/codyco:/usr/local/share/yarp:/usr/local/share/iCub:/home/cai/share/ICUBcontrib:/home/cai/share/iCub
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/cai/share

#----------------------------------------
echo "Welcome $YARP_ROBOT_NAME!"

caicaixia · 2018-01-26T03:28:27Z

The error seems like, it try to call /codyco-superbuild/build/install/bin/qpOASES.m. However, I opened this file, there is no codes inside, only some help description. Is this correct?

I can find my libqpOASES.so in /home/cai/Workspace/codyco-superbuild/build/install/lib, but when I try to locate it locate libqpOASES.so in terminal, there is no out put.

I already put this line in .bashrc: export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$CODYCO_SUPERBUILD_ROOT/build/install/lib

I think this maybe the problem about qpOASES. On matlab, I am reading:

>> which qpOASES
/home/cai/Workspace/codyco-superbuild/build/install/bin/qpOASES.m
>> which -all qpOASES
/home/cai/Workspace/codyco-superbuild/build/install/bin/qpOASES.m
/home/cai/Workspace/codyco-superbuild/build/install/mex/qpOASES.mexa64  % Shadowed
>>

In the install folder, the mex files have the number 3.1, not the new version 3.2:

caicaixia · 2018-01-26T07:07:52Z

Hello @gabrielenava , I solved the qpOASES errors.
In matlab configuration, you ask me to add those two to the MATLAB PATH:

    addpath(['codyco_superbuild_install_folder'  /mex])
    addpath(genpath(['codyco_superbuild_install_folder'  /share/WB-Toolbox]))

But actually I added the all superbuild/build_folder/install to matlab path. So when the codes require the qpOASES, it will find the /codyco-superbuild/build/install/bin/qpOASES.m first instead of /codyco-superbuild/build/install/mex/qpOASES.mexa64
`
So if only add that two subfolders to matlab path, it has no errors anymore. Sorry for that.

Now I can run demo torqueBalancingYoga , but the motion is very very slow, and the iCub only left one leg for one side, then stop there.

I saw the videos on youtube yoga, the iCub lifts two side of legs one by one, and the motions are also fast.

Is it support to be which one? How can I increase the speed?

gabrielenava · 2018-01-26T11:22:33Z

Don't worry @caicaixia, happy to hear that the controllers are now working!

Regarding the yoga on both feet, there are a bunch of options in this file:

https://github.com/robotology-playground/wholeBodyControllers/blob/master/controllers/torqueBalancingYoga/app/robots/icubGazeboSim/initStateMachineYoga.m#L127

in particular, if you set to True the option Sm.yogaAlsoOnRightFoot, the robot will perform the Yoga demo on both feet.

Regarding the velocity of the simulation, the parameter to check is the time inside the Simulink controller, because the real time factor parameter inside Gazebo will be 0 if you correctly synchronize Gazebo with Simulink.

Unfortunately, the speed strongly depends on the performances of your PC. On my personal laptop (that is not incredibly powerful) I can run the Yoga demo more or less with a Real Time Factor of 0.5.

If the demo is reeeeeally slow, it is possible that you compiled some repos (Yarp, icub-main, codyco-superbuild) in debug mode instead of release. You can double check this by running ccmake ../ in the build folders.

If instead your real time factor is good (> 0.5) and you just want to increase the speed of the Yoga movements, this is the parameter to decrease (it is the time the robot needs to move smoothly from one yoga position to the next):

https://github.com/robotology-playground/wholeBodyControllers/blob/master/controllers/torqueBalancingYoga/app/robots/icubGazeboSim/initStateMachineYoga.m#L83

if you change this parameter (or others), it is possible that you need a bit of gain tuning to have the demo working. All parameters to tune are inside the file I already linked, https://github.com/robotology-playground/wholeBodyControllers/blob/master/controllers/torqueBalancingYoga/app/robots/icubGazeboSim/initStateMachineYoga.m

caicaixia · 2018-01-29T03:04:24Z

Hello @gabrielenava Thanks for explanation. Yes, my codyco-superbuild was in debug. Then I changed it to release and run again. I found my default Real Time Factor in Gazobo at beginning is around 0.85(before run), then when it's running, it becomes 0. Then the robot went crazy, it cannot do the yoga :(

Here is the errors:

ERROR:  Premature homotopy termination because QP is infeasible
->ERROR:  Initial QP could not be solved due to infeasibility!
ERROR:  Premature homotopy termination because QP is infeasible
->ERROR:  Initial QP could not be solved due to infeasibility!

I also test the demo torqueBalancingStandup, its also out of control.
Is this because my default Real Time Factor is too high? How can I change this number?

Notes: I didn't change any parameters by myself.

gabrielenava · 2018-01-29T11:56:02Z

Regarding:

ERROR:  Premature homotopy termination because QP is infeasible
->ERROR:  Initial QP could not be solved due to infeasibility!
ERROR:  Premature homotopy termination because QP is infeasible
->ERROR:  Initial QP could not be solved due to infeasibility!

this is an output of the quadratic programming solver qpOASES which is used inside the controller. It tries to find the optimal set of feet contact forces (given the unilateral and friction cone constraints), so if the robot falls it will trow an error saying that it couldn't find a feasible solution. This is normal, and it is just a result of the fact the robot fell down in the simulator. Don't worry about that.

The real time factor is fine. Unfortunately, the result of the simulation is affected by the way the physics engine in Gazebo solves the rigid body dynamics. If you change the physics engine in Gazebo the results may change. This is my setup in gazebo:

However, even if your setup is the same there may be different results depending on your laptop performances. This is a known issue due to the presence of some filters on the velocity measurements (not at the controller level). See also robotology/wb-toolbox#26.

The new WBT3.0 should fix the issue, meanwhile you can try the following:

reduce/increase the gains that multiply velocity terms. For the yoga, is this parameter: https://github.com/robotology-playground/wholeBodyControllers/blob/master/controllers/torqueBalancingYoga/app/robots/icubGazeboSim/initStateMachineYoga.m#L52. You can start multiplying/dividing that gains by 2 and see how the robot behaves.
if this can't solve the issue, try to post here a video of the moment the robot is falling, so that I can help you to tune the other parameters.

caicaixia · 2018-01-30T09:04:03Z

Hello @gabrielenava, I checked my Gazebo physics engine, the setup is exactly the same as yours. But its still falling.

I also tried to turn the PD parameters in initStateMachineYoga.m. I tried many possible cases but all are falling :(

Video:
Videos.zip

QA:
I want to double check, in torqueBalancingYoga: You said: The corresponding home position is homePoseYogaPP. Does that mean:
$ yarpmotorgui --from homePoseBalancing.ini -> Global Joints Commands/Custom postions/Moveallpart to YogaPP ??

gabrielenava · 2018-01-30T13:52:05Z

QA:
I want to double check, in torqueBalancingYoga: You said: The corresponding home position is homePoseYogaPP. Does that mean:
$ yarpmotorgui --from homePoseBalancing.ini -> Global Joints Commands/Custom postions/Moveallpart to YogaPP ??

This is correct :-)

Looking at the videos, it seems to me that there is something strange, especially in the second one. Can you output the result of yarp resource --find model.urdf? Just to verify the model the controller is taking is correct (it is not the .sdf that is used for Gazebo, but they should be equivalent)

Other two parameters to change are Reg.PinvDamp and Reg.PinvDamp_nub. Try to increase Reg.PinvDamp_nub (up to 1e-3) and play with the other as for the gains. Remember it is always better to modify one parameter at a time.

gabrielenava · 2018-07-05T08:35:10Z

closing. This documentation is deprecated: we now use the robotology-superbuild instead of the codyco-superbuild.

caicaixia · 2018-07-05T08:56:26Z

Looking at the videos, it seems to me that there is something strange, especially in the second one. Can you output the result of yarp resource --find model.urdf? Just to verify the model the controller is taking is correct (it is not the .sdf that is used for Gazebo, but they should be equivalent)

Sorry I didnt replied your last question, since that time, I already gave up that simulation.
I just checked:

 yarp resource --find model.urdf
/home/xxx/Workspace/codyco-superbuild/build/install/share/codyco/robots/icubGazeboSim/model.urdf

Not sure if it correct or not?

dosh1234 · 2018-07-30T07:22:01Z

Hi! @caicaixia .When I used ICub robot iCubGazeboV2_5. I want to do the walking simulation of a robot under gazebo， I used gazebo8 ， installed iCub-models, run:
$ export YARP_ROBOT_NAME=icubGazeboSim $ yarpserver $ gazebo -slibgazebo_yarp_clock.so, then insert an iCub model (iCubGazeboV2_5)
then run
YARP_CLOCK=/clock yarprobotinterface --config launch-wholebodydynamics.xml
I got some problems.

yarp: Port /dosh-Lenovo-V310-15IKB/no_progname/16160/clock:i active at tcp://192.168.199.141:10201/
Success: port-to-port persistent connection added.
yarp: Waiting for clock server to start broadcasting data ...
yarp: Receiving input from /clock to /dosh-Lenovo-V310-15IKB/no_progname/16160/clock:i using tcp
||| configuring
||| default config file specified as yarprobotinterface.ini
||| checking [/home/dosh/yarprobotinterface.ini] (pwd)
||| checking [/usr/local/share/yarp/robots/iCubGazeboV2_5/yarprobotinterface.ini] (robot)
||| checking [/home/dosh/soft/icub-models/share/iCub/robots/iCubGazeboV2_5/yarprobotinterface.ini] (robot)
||| checking [/home/dosh/.config/yarp/yarprobotinterface.ini] (YARP_CONFIG_HOME)
||| checking [/home/dosh/.local/share/yarp/yarprobotinterface.ini] (YARP_DATA_HOME)
||| checking [/etc/xdg/xdg-ubuntu/yarp/yarprobotinterface.ini] (YARP_CONFIG_DIRS)
||| checking [/usr/share/upstart/xdg/yarp/yarprobotinterface.ini] (YARP_CONFIG_DIRS)
||| checking [/etc/xdg/yarp/yarprobotinterface.ini] (YARP_CONFIG_DIRS)
||| checking [/usr/local/share/yarp/yarprobotinterface.ini] (YARP_DATA_DIRS)
||| checking [/home/dosh/soft/icub-models/share/iCub/yarprobotinterface.ini] (YARP_DATA_DIRS)
||| found /home/dosh/soft/icub-models/share/iCub/yarprobotinterface.ini
||| finding file [config]
||| checking [/home/dosh/launch-wholebodydynamics.xml] (pwd)
||| checking [/usr/local/share/yarp/robots/iCubGazeboV2_5/launch-wholebodydynamics.xml] (robot)
||| checking [/home/dosh/soft/icub-models/share/iCub/robots/iCubGazeboV2_5/launch-wholebodydynamics.xml] (robot)
||| checking [/home/dosh/.config/yarp/launch-wholebodydynamics.xml] (YARP_CONFIG_HOME)
||| checking [/home/dosh/.local/share/yarp/launch-wholebodydynamics.xml] (YARP_DATA_HOME)
||| checking [/etc/xdg/xdg-ubuntu/yarp/launch-wholebodydynamics.xml] (YARP_CONFIG_DIRS)
||| checking [/usr/share/upstart/xdg/yarp/launch-wholebodydynamics.xml] (YARP_CONFIG_DIRS)
||| checking [/etc/xdg/yarp/launch-wholebodydynamics.xml] (YARP_CONFIG_DIRS)
||| checking [/usr/local/share/yarp/launch-wholebodydynamics.xml] (YARP_DATA_DIRS)
||| checking [/home/dosh/soft/icub-models/share/iCub/launch-wholebodydynamics.xml] (YARP_DATA_DIRS)
||| checking [/usr/share/iCub/launch-wholebodydynamics.xml] (YARP_DATA_DIRS)
[DEBUG]Reading file
[FATAL]Syntax error while loading at line 0 . Failed to open file
yarprobotinterface() [0x406420]
yarprobotinterface() [0x437a78]
yarprobotinterface() [0x41b55d]
/usr/lib/x86_64-linux-gnu/libYARP_OS.so.1(_ZN4yarp2os8RFModule9runModuleERNS0_14ResourceFinderE+0x62) [0x7fb4a8dfb4d2]
yarprobotinterface() [0x405b2a]
/lib/x86_64-linux-gnu/libc.so.6(__libc_start_main+0xf0) [0x7fb4a8388830]
yarprobotinterface(_start+0x29) [0x405d89]
dosh@dosh-Lenovo-V310-15IKB:~$

I thought I miss launch-wholebodydynamics.xml. I search it in my computer, can't found it.Can you help me about this question. Thank you！

gabrielenava added the how-to label Jan 19, 2018

traversaro mentioned this issue Jan 26, 2018

Investigate role of qpOASES.m and if it should be installed robotology-dependencies/qpOASES#6

Closed

gabrielenava changed the title ~~Pointers to the documentation for running torque control on iCub~~ How to setup the enviroment for running torque control on iCub Feb 2, 2018

gabrielenava mentioned this issue Feb 4, 2018

ERROR: Premature homotopy termination because QP is infeasible ->ERROR: Initial QP could not be solved due to infeasibility! #14

Closed

gabrielenava closed this as completed Jul 5, 2018

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

How to setup the enviroment for running torque control on iCub #10

How to setup the enviroment for running torque control on iCub #10

gabrielenava commented Jan 19, 2018

caicaixia commented Jan 24, 2018 •

edited

Loading

diegoferigo commented Jan 24, 2018 •

edited

Loading

caicaixia commented Jan 24, 2018 •

edited

Loading

gabrielenava commented Jan 24, 2018 •

edited

Loading

gabrielenava commented Jan 24, 2018

caicaixia commented Jan 25, 2018

diegoferigo commented Jan 25, 2018

caicaixia commented Jan 25, 2018 •

edited

Loading

gabrielenava commented Jan 25, 2018

caicaixia commented Jan 26, 2018 •

edited

Loading

caicaixia commented Jan 26, 2018 •

edited

Loading

caicaixia commented Jan 26, 2018

gabrielenava commented Jan 26, 2018 •

edited

Loading

caicaixia commented Jan 29, 2018 •

edited

Loading

gabrielenava commented Jan 29, 2018 •

edited

Loading

caicaixia commented Jan 30, 2018

gabrielenava commented Jan 30, 2018

gabrielenava commented Jul 5, 2018

caicaixia commented Jul 5, 2018

dosh1234 commented Jul 30, 2018

How to setup the enviroment for running torque control on iCub #10

How to setup the enviroment for running torque control on iCub #10

Comments

gabrielenava commented Jan 19, 2018

This issue is for collecting all the links to the documentation which is necessary to follow in order to run torque control simulations with iCub.

Basic requirements:

Installing yarp, icub-main and codyco-superbuild

Case 1: install yarp and icub-main outside the codyco-superbuild

Case 2: install yarp and icub-main inside the codyco-superbuild

Install the codyco-superbuild

What is the codyco-superbuild?

What should I do to install it?

Download the whole body torque control repository and its dependencies

Remark 1

Remark 2

Things are still not working: where should I open issues?

caicaixia commented Jan 24, 2018 • edited Loading

diegoferigo commented Jan 24, 2018 • edited Loading

caicaixia commented Jan 24, 2018 • edited Loading

gabrielenava commented Jan 24, 2018 • edited Loading

gabrielenava commented Jan 24, 2018

caicaixia commented Jan 25, 2018

diegoferigo commented Jan 25, 2018

caicaixia commented Jan 25, 2018 • edited Loading

gabrielenava commented Jan 25, 2018

caicaixia commented Jan 26, 2018 • edited Loading

caicaixia commented Jan 26, 2018 • edited Loading

caicaixia commented Jan 26, 2018

gabrielenava commented Jan 26, 2018 • edited Loading

caicaixia commented Jan 29, 2018 • edited Loading

gabrielenava commented Jan 29, 2018 • edited Loading

caicaixia commented Jan 30, 2018

gabrielenava commented Jan 30, 2018

gabrielenava commented Jul 5, 2018

caicaixia commented Jul 5, 2018

dosh1234 commented Jul 30, 2018

caicaixia commented Jan 24, 2018 •

edited

Loading

diegoferigo commented Jan 24, 2018 •

edited

Loading

caicaixia commented Jan 24, 2018 •

edited

Loading

gabrielenava commented Jan 24, 2018 •

edited

Loading

caicaixia commented Jan 25, 2018 •

edited

Loading

caicaixia commented Jan 26, 2018 •

edited

Loading

caicaixia commented Jan 26, 2018 •

edited

Loading

gabrielenava commented Jan 26, 2018 •

edited

Loading

caicaixia commented Jan 29, 2018 •

edited

Loading

gabrielenava commented Jan 29, 2018 •

edited

Loading