robotology / whole-body-controllers

Simulink-based whole body controllers for humanoid robots.

whole-body-controllers

Warning! This repository contains reseach material and therefore is under active development. In future releases, master branch may break compatibility with older versions of WBC. If you are interested in retrieving a stable version of this repo, fork the repository or refer to the following releases:

• WBC v2.5
• WBC v2.0
• WBC v1.5
• WBC v1.0

Overview

The repository contains Simulink-based whole-body controllers developed to control the iCub humanoid robot. It can be imagined as a starting point and a support repository for a user that intends to develop a new Simulink controller (not necessarily for the iCub robot) in within the framework of the robotology organization. It is worth noting that:

• The controllers stored in this repository are an overview of the possibile control frameworks that can be implemented using the robotology software infrastructure. Also, the repository contains a library of configuration and utility Matlab functions to design simulations with Gazebo simulator and on the real robot iCub. With the dependency matlab-whole-body-simulator installed, you can also design simulations with a full MATLAB/Simulink simulator and robot visualizer, accessible through the Simulink Library Browser entry Matlab Whole-body Simulator (refer to this README).

• The robot dynamics and kinematics is computed run-time by means of WBToolbox, a Simulink library that wraps iDyntree. For more information on iDyntree library, see also this README.

• The Simulink models implement different control strategies both for fixed-base and for floating-base robots. They space from momentum-based torque control to inverse-kinematics-based position control. Have a look at the controllers folder for more details.

Dependencies

This repository depends upon the following Software:

• CMake, at least version 3.5.
• Matlab/Simulink, default version R2019b.
• WB-Toolbox and blockfactory.
• matlab-whole-body-simulator, at least version 2.0.0.
• Gazebo Simulator, default version 9.0.
• gazebo-yarp-plugins.
• icub-gazebo, icub-gazebo-wholebody and icub-models to access iCub models.
• whole-body-estimators (Optional, for using wholeBodyDynamics device).
• YARP and icub-main.

Installation and usage

The repository is usually tested and developed on Ubuntu and macOS operating systems. Some functionalities may not work properly on Windows.

• It is suggested to install whole-body-controllers and most of its dependencies (namely, YARP, icub-main, whole-body-estimators,icub-gazebo,icub-gazebo-wholebody, icub-models, gazebo-yarp-plugins, matlab-whole-body-simulator, blockfactory and WB-Toolbox and their dependencies) using the robotology-superbuild (enable ROBOTOLOGY_ENABLE_DYNAMICS option). Warning: the superbuild can download and compile the repository also without having Matlab, Simulink and Gazebo installed in the PC, but the functionalities of the repo will be considerably reduced! To access all the features of the repo, install all the dependencies. Also, in the superbuild you have to enable the ROBOTOLOGY_USES_GAZEBO and ROBOTOLOGY_USES_MATLAB options.

• Otherwise, after installing all the dependencies, clone the repository on your pc by running on a terminal git clone https://github.com/robotology/whole-body-controllers, or download the repository. Then (on Ubuntu), open a terminal from the folder where you downloaded whole-body-controllers and run:

  mkdir build
  cd build
  ccmake ..
  

  in the GUI that it will open, set the CMAKE_PREFIX_PATH as your desired installation folder. Then, run make install.

• Set the environmental variable YARP_ROBOT_NAME in your .bashrc file (or equivalent) to be the name of the robot you want to control. List of supported robot names:

  Robot Names	Associated URDF Model
  iCubGenova02	model.urdf
  iCubGenova04	model.urdf
  iCubGazeboV2_5	model.urdf
  icubGazeboSim	model.urdf

• IMPORTANT! to use the WBC Simulink controllers, it is required to add the installed +wbc and +wbc/simulink folders (copied from +wbc) to the Matlab path. There are two possible ways to add the folder to the Matlab path:

  1a. manually and permanently add the parent folder of the installed +wbc (${CMAKE_INSTALL_PREFIX}/mex), and its sub-folder +wbc/simulink to the Matlab path;

  1b. run only once the startup_WBC.m script, which is installed in your ${BUILD} folder. In this case, path is not permanently added to Matlab, and it is required to always start Matlab from the folder where your pathdef.m file is (usually ~/Documents/MATLAB). To facilitate the reaching of the WBC working folder from the folder containing the pathdef.m, a goToWholeBodyController.m script can be automatically created in that folder. Run it to jump to the WBC folder. For further information on the installation procedure see also the WBToolbox documentation. WARNING: if the repository is installed through the robotology-superbuild, DO NOT run the startup_WBC.m file but instead run the startup_robotology_superbuild file that comes along with robotology-superbuild installation.

  • Note: to use any function inside the package matlab-wbc/+wbc, add the wbc prefix to the function name when the function is invoked, i.e. [outputs] = wbc.myFunction(inputs). More information on packages can be found in the Matlab documentation.

• The folder ${CMAKE_INSTALL_PREFIX}/mex having been already added to the Matlab path, there are no other requirements for using the simulation library from matla-whole-body-simulator.

• There are some functionalities of the repo such as the automatic generation of c++ code from Simulink that require to enable not-default cmake options. Check the available options by running ccmake . in your build directory.

Troubleshooting

Please refer to the WBToolbox troubleshooting documentation.

Relevant folders of the repo

• config: a collection of scripts to correctly configure this repo. [README]

• controllers: Simulink whole-body position and torque controllers for balancing of humanoid robots. [README]

• doc: guidelines on how to create/use Simulink models for control. [README]

• library: a library of functions/scripts used by the controllers. [README]

• utilities: Simulink models for debugging sensors on the real robot. [README]

Available controllers

• fixed-base-joints-torque-control
• floating-base-balancing-position-control
• floating-base-balancing-torque-control
• floating-base-balancing-torque-control-with-simulator
• floating-base-jerk-control

Matlab functions library

• matlab WBC library

Additional features

Automatic generation of c++ code from Simulink

There is the possibility to generate c++ code from the Simulink models using Simulink coder (available only for the floating-base-balancing-torque-control). The repositiory that contains the generated c++ code is named autogenerated-whole-body-controllers. Documentation on how to generate the code is available in the repository wiki.

Static GUI for Simulink

When used for controlling real platforms, heavy Simulink models may violate the user-defined simulation time step, see also this issue. It seems a source of delay is the run-time update of the Simulink interface. For this reason, a static GUI for running the models has been developed. If you want to run Simulink with the static GUI, run the startModelWithStaticGui script.

Home positions for yarpmotorgui

The repo contains a set of predefined home positions to be used with the yarpmotorgui. By default, if the repo is installed through robotology-superbuild, the home positions are installed in the robotology-superbuild/build/install directory. Otherwise add the path to the homePositions folder to the YARP_DATA_DIRS environmental variable in your .bashrc file . The command to use the home positions with the yarpmotorgui is yarpmotorgui --from myHomePosFileName.ini.

Where do I find legacy materials?

Official legacy repositories are: mex-wholebodymodel and WBI-Toolbox-controllers. Note: these legacy repos contain undocumented/outdated code, and duplicated or not tested matlab functions. They also contain original code that has been tested on the robot in the past and then never used again, or code that will be ported in the main repository in the future.

• exploit friction controller
• walking controller
• seesaw controller
• automatic gain tuning and automatic gain tuning-matlab
• elastic joints control
• walkman control and walkman control-matlab
• joint-space control and centroidal transformation
• stand-up control 4 contacts

You can also find other legacy controllers/simultors in this repository whole-body-controllers in specific commits:

• simulink-balancing-simulator@c217f051.

Citing this work

If you are using this code for your research activity and you're willing to cite it, you may add the following references to your bibliography:

  @INPROCEEDINGS{Nava_etal2016,
  author={G. Nava and F. Romano and F. Nori and D. Pucci}, 
  booktitle={2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS)}, 
  title={Stability analysis and design of momentum-based controllers for humanoid robots}, 
  year={2016}, 
  pages={680-687}, 
  doi={10.1109/IROS.2016.7759126}, 
  month={Oct},
  }

  @article{Nori_etal2015,
  author="Nori, F. and Traversaro, S. and Eljaik, J. and Romano, F. and Del Prete, A. and Pucci, D.",
  title="iCub whole-body control through force regulation on rigid non-coplanar contacts",
  year="2015",
  journal="Frontiers in {R}obotics and {A}{I}",
  volume="1",
  }

Mantainers

Main: Gabriele Nava (@gabrielenava)

Auxiliary: Giuseppe L'Erario (@giulero)

whole-body-controllers's Issues

Cleanup wholeBodyDynamics port names

There is a bit of confusion in the definition of the port names for reading the external forces acting on the robot. In particular:

• the port that reads the forces acting at the robot feet is called /wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o (same for the left foot). Only for iCubGenova04, the port has been renamed right_foot, and still the string variable that stores the port name in Matlab is Ports.WBD_RIGHTLEG_EE.

• the port that reads the forces acting at the robot hands is called /wholeBodyDynamics/right_arm/endEffectorWrench:o. The string variable that stores the port name in Matlab is Ports.RIGHT_ARM.

cc @Yeshasvitvs

Bug in the Simulink GUI

There is a bug in the Simulink GUI. If the user presses the stop button twice sufficiently fast, the controller is restarted. I will update and fix this issue. Possible solution: have separated start/stop buttons

Create diagrams of the momentum-based controller

To be added to the READMEs

[Archived knowledge] Matlab crashes when opening Simulink scopes

I had issues on Ubuntu 18.04 LTS with Matlab 2018a: very often, after running a Simulink simulation with Gazebo, Matlab crashes after trying to open a scope.

I fixed the issue by running Matlab from terminal as follows:

matlab -softwareopengl

Update README to include link to autogenerated code repo

Fix broken links in main README

There are many broken links in the main README. Especially in the section Where do I find new features and legacy repos?.

Here is a list of the ones that I have found:

• exploit friction and walking controller
• all links in Legacy
• idyntree-high-level-wrappers

Update Yoga ++ demo

I need to modify some joints references for the postural task, in order not no hit the robot's joints limits. Furthermore, a new demo must be designed. The robot has to perform a "slow" yoga, followed by a "fast" yoga, while standing on the one foot. It may be the occasion for solving also this TODO.

Implement new logic for activation/deactivation while walking with MPC

For the moment, the MPC is sending the contact status: a vector of 1 and 0 depending if the corresponding contact is activated (1) or not (0). However, contact activation is not always triggered correctly, and this results in discontinuities and affects the controller accuracy. The simplest test to do is to use a threshold on the vertical force at feet in order to detect contact activation/deactivation.

Add the integration-based IK and the CMM blocks to the library

There are two useful Simulink blocks I've implemented that are over-used in several controllers, also not in this repo. The two blocks are the one performing integration-based IK and the one for computing the Centroidal Momentum Matrix. I would like to add these blocks to the library folder in this repo.

Shall we use Travis for dealing with different Simulink versions?

It has always been a problem to deal with different Simulink versions because a model created with a newer version cannot be opened using an older version. Same happens if a model created with an old version is opened with a newer Simulink.

In WBIToolbox-Controllers the proposed solution was to duplicate each model so that it is possible to open it with different Simulink versions. I don't like this solution for two reasons:

• It is difficult to mantain all models even if the exporting procedure is straightforward. Furthermore, users are often confused because they do not know which model to run;
• If I'm a user of an old Simulink version, there is no way for me to know if the model I'm using is the most updated one.

In this repo, I came up with a simpler solution: all models are written in 2016b which is the Matlab version that is used inside the lab. Plus, a legacy version of the models is saved in the lowest supported version, i.e. Matlab 2014a.

Why not save directly the models in version 2014a?

Because this implies the user must run the export script every time he need to push a modification in the repo, and I do not trust the user (e.g., I already pushed one commit using Matlab 2017b because I forgot to export once).

So, what we can do?

A solution (proposed by @diegoferigo) can be: the user can push any version he wants. Then, Travis can automatically export all models into version 2014a. But I do not really know if this is feasible.

cc @traversaro @diegoferigo

MPC controller won't work if rotational task frame is rotated w.r.t. feet frames

In order to make the robot able to turn in simulation, the reference orientation for the rotational task is updated according to the feet orientation. However, this solution fails when the frame representing the position and orientation of the link chose for the rotational task is oriented differently from the feet frames. Another solution is therefore required to solve the bug.

Improve the visualization and data collection

Currently, the Simulink controllers are endowed with a Visualization block (see pictures), that contains several Scopes allowing the user to visualize several control-related quantities (center of mass position, joint torques, ...). It is also possible to dump all these plots in the Matlab workspace and save them in a file.

However, there are a couple of limitations:

• The visualization of joints-related quantities (joint positions, velocities, torques, ...) relies on the DoFs Converter block from WBToolbox. This block is the only reason why, in order to compile the Simulink model, it is required to be able to connect with the robot. Also the block is currently not working fine: instead of dividing the input vector in sub-vectors according to the number of controlboards (right arm, left leg, ...), it creates a set of vectors with the same length of the input, whose values are zeros but the ones corresponding to the elements of the input vector that belong to each sub-part of the robot. It would be nice to remove this block from WBToolbox and implement something directly in Simulink instead.

• There is no an automatic way to plot the data saved on the workspace. @ahmadgazar did something for this: ahmadgazar@94886ca

cc @diegoferigo

Add emergency stop if the encoders ratio is bigger than a threshold

In case the joints encoders have spikes, it would be nice to have an automatic emergency stop if the encoders ratio is bigger than a threshold. To be implemented in the current release.

Release the controllers with WB-Toolbox 3 support

Following robotology/community#279 and #5, the controllers stored in the WBT3 branch can be merged if they're ready.

Improve documentation on How-to-run-torqueBalancing-on-real-iCub

Some passages of the How-to-run-torqueBalancing-on-real-iCub are missing or not clear for new users.

• The SIMULATION reference in the title is tricky (it is the same as How-to-run-torqueBalancing-simulations-with-iCub to work in simulation with Gazebo)

• The YARP_ROBOT_NAME is set in the init function (e.g. InitTorqueBalancingYoga), so it has to be changed accordingly to the robot one wants to use.

• It is not specified which passages have to be executed with the robot on-flight or on-the-ground. (e.g. custom position -> on-flight, run the module -> on-gorund)

@gabrielenava @traversaro

Migration to WBToolbox 3.0

For the moment, blocked by #2.

How to setup the enviroment for running torque control on iCub

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.

Error evaluating `InitFcn` callback of block_diagram `torqueBalancingR2015bXsens`

I'd like to control all the joints of the iCub included the wrist and the head.
I changed the parameters in gains.m but I get this error. What else do I have to change?

Error evaluating 'InitFcn' callback of block_diagram 'torqueBalancingR2015bXsens'.
Caused by: Dimensions of matrices being concatenated are not consistent.

[Archived knowledge]: Matlab is unable to open MatlabWindow application

It happened to me with Ubuntu 18.04 and Matlab 2018a. The instruction provided in this link solved the issue: https://www.mathworks.com/matlabcentral/answers/364551-why-is-matlab-unable-to-run-the-matlabwindow-application-on-linux

Update all models to launch Simulink from command line

In view of robotology/wb-toolbox#160 and #38, it is necessary to modify all the startup procedure of the Simulink controllers. The idea is to create a Matlab function named startTorqueBalancingxxx.m and the open a (static) gui that can handle the Simulink model without opening it.

The size of this repo is increasing

At the time of writing 11.8 Mb.

I think this is impossible to avoid. Sometimes it is necessary to modify/add .mdl files. However, maybe my legacy folder is contributing to increasing the size of this repo. Maybe the suggestion in this issue can help.

Usage of "model" block for creating a Simulink library for controllers

In the past (1 month ago), I tried to use the model block to create a Simulink library for all the controllers. The idea was to avoid duplication of blocks which are shared between different controllers. I had some difficulties to compile referenced models that contains S-functions (almost all of them), but I can try again with a simple controller such as wholeBodyImpedance and see if it is possible to use it.

Write Documentation of the repo

A readme and a small guide for running the controllers are required.

Boost data processing for data coming from the Simulink model

With this commit, I added a callback in every model to a function like this one. The function is called every time the terminate button is pressed. The purpose was to remove the local paths that are added when running a specific simulation, e.g. Yoga or Standup. The reason is that the local paths may contain functions with the same names and this may be dangerous (it is not clear which function is called).

However, I noticed that this callback function is run only after the to workspace block updates the workspace. I realized that we can then use this function to plot data, save the workspace, etc, automatically and every time we run a demo on the robot. This may be useful for processing data while on the robot, or compare two different simulations and decide if the data collected are valuable.

[WARNING]: WBC library of matlab functions will be transformed into a package in the future release

In the current master branch, there is a library of matlab utility functions that are used by the controllers. However, we have other controllers in other github organizations, including matlab-simulators that use local copies of these functions. I decided to convert this library in a package in order to:

• avoid to duplicate the library in several repositories;
• allow to permanently add the library to the Matlab path. If I don't use the package, it may be difficult to understand which function is used in case there are functions with the same name in other folders in the path (there may be random failures, e.g. imagine you have your local version of the skew function in your local path. If for some reason the path to your local version of skew is not loaded, and the library of whole-body-controllers is in the Matlab path, your code may run anyways, but unexpected results may occur.

The modification has been implemented and tested in this PR. When it will be merged into master, all the standard demos (yoga, standup, impedance) should work as before. All other not officially supported demos can work as before as well, provided that the user adds the prefix wbc. to the name of all the functions of the library when there is a call to these functions. E.g.:

function [ ] = myRandomFunction ()

   % call to one of the functions in the library (add wbc.)
   [outputs] = wbc.nameOfFunctionInsideTheLibrary(inputs)
end

cc @Yeshasvitvs @aikolina

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

I edited the simulink scheme of the controller torqueBalancing in https://github.com/robotology-playground/WBI-Toolbox-controllers. I'm using the SM COORDINATOR. I'm sending through 2 yarp ports the jointAngles and the CoM coordinates wrt to l_sole, respectively. The motion is a simple motion with the arms with the 2 feet on the ground, that works if retargeted on the old C++ torqueBalancing even if a bit unstable.
In this case I edited the simulink in this way:

These are the signals at the beginning of the simulation when the robot doesn't move and waits for the commands from the read ports:

When sending the references these are the signals from the probes:

the robots first moves toward the right foot and then falls down giving then the error.
I'm using the following gains:

Introducing an extreme smoothing the situation is the following:


The robot does not fall but does the exact same movemnet as before toward the right foot but if I waited for way longer I'm sure it would have fallen, generating the error.
Indeed the simulation on gazebo runs extremely slow even when gazebo is running on another computer, the real time factor goes even to 0 sometimes.

Update whole-body-controllers

A general update is required to address several issues and to properly deal with new features such as the autogenerated Yoga++. In particular, this is the list of issues that is necessary to address:

This repo

• New features and legacy
• Improve the visualization and data collection
• Get YARP_ROBOT_NAME from the system instead of overwriting it
• Use QP block from WBToolbox in the controllers
• Cleanup wholeBodyDynamics port names

Loc2

• Use different notation for the centroidal momentum
• [ ] Test auto-generated Yoga++ running from icub-head of iCubGenova04

Include motor reflected inertia in the balancing controller

References:

Exploiting Friction in Torque Controlled Humanoid Robots, 2018 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS) Madrid, Spain, October 1-5, 2018

Issue recap: how to add motor reflected inertia with coupled joints to the rigid body system dynamics

Consider the following equations of motion for a humanoid robot:

where (1) represents the floating base dynamics, (2) is the joint space dynamics, and (3) is the motors dynamics. The above equations represent a simplified model for the robot-motor dynamics under the following assumptions

• The angular motor kinetic energy is due to its own spinning only, and the center of masses of each motor is along the motor axis of rotation;
• Both stiffness and damping of the series elastic actuators can be considered linear versus its relative displacement absolute value and rate-of-change.
• All motors are rigidly connected to the transmission element.

Now, focus on Eq. (3):

• Γ is a diagonal matrix containing the gear box ratios;
• T is a block diagonal matrix, representing the the coupling between the input and the output rotations of the coupling mechanism;
• I_m is a diagonal matrix containing the motor inertias.

Then, as explained here, the kinematic relation

induces the following relationship between joint torques and the torques transmitted to the motor after gear box and coupling:

The above justifies the second term on the right hand side of Eq. (3).

The above kinematic relation, instead, induces the following relationship between motors and joint accelerations:

By substituting this last expression into (3) and multiplying by transpose((TΓ)^-1), one has:

Finally, by summing up (4) with (2) one has:

where:

Now, the system (5)-(6) can be solved using u as control input, and applying e.g. a classical momentum-based approach. Then, the control input u can be related to the actual joint torques using (4) as follows:

where K_ff∈ [0,1].

Documentation on how to create matrix T:

http://wiki.icub.org/wiki/ICub_coupled_joints

Implemented in the controllers with this commit.

Move to robotology

If you are ok with it @gabrielenava , we can now move this repo to the robotology organization.

Porting to the new WBT v5

Here is the reference issue in WB-Toolbox:

robotology/wb-toolbox#167

The porting will be performed in a devel branch.

Automatic data processing and plotting of whole-body datasets

Following #13, it would be really nice having a matlab class or script that automatically processes data logged from Simulink (initial idea here). This would require introducing a generic structure for storing whole-body data.

In this way, if the visualization section of all controllers is the same (better if shared), creating plots for publications and documentation would become trivial. We know well how much time we spend on this.

cc @gabrielenava @S-Dafarra

Check mex-wholebodymodel status and eventually port the code into wholeBodyControllers

mex-wholeBodyModel will be used by @ahmadgazar for simulating iCub and Walkman with SEA. It is therefore necessary to check if the code still compiles and eventually port it into this repo.

Compilation broken on master branch

I tried to compile the repo using the robotology-superbuild. I reverted yarp and icub-main to master branch. I got the following error:

Not searching for unused variables given on the command line.
loading initial cache file /home/gnava/Software/github/robotology/robotology-superbuild/build/robotology/whole-body-controllers/CMakeFiles/YCMTmp/whole-body-controllers-cache.cmake
CMake Error at cmake/RegisterMdl.cmake:60 (message):
  Folder torqueBalancingYoga_grt_rtw not found.
Call Stack (most recent call first):
  torque-controllers/momentum-based-yoga/CMakeLists.txt:1 (register_mdl)


-- Configuring incomplete, errors occurred!
See also "/home/gnava/Software/github/robotology/robotology-superbuild/build/robotology/whole-body-controllers/CMakeFiles/CMakeOutput.log".
CMakeFiles/whole-body-controllers.dir/build.make:103: recipe for target 'robotology/whole-body-controllers/CMakeFiles/YCMStamp/whole-body-controllers-configure' failed
make[2]: *** [robotology/whole-body-controllers/CMakeFiles/YCMStamp/whole-body-controllers-configure] Error 1
CMakeFiles/Makefile2:1103: recipe for target 'CMakeFiles/whole-body-controllers.dir/all' failed
make[1]: *** [CMakeFiles/whole-body-controllers.dir/all] Error 2
Makefile:94: recipe for target 'all' failed
make: *** [all] Error 2

cc @diegoferigo

Switching between different demos in task-based controller is complicated

The issue is that the MPC walking controller needs a port to have the blocking read activated:

but this is preventing the "example state machine" demo to start. If one removes the blocking read for testing the controller without MPC, then it is difficult to remember to activate the blocking read again.

torqueBalancingYoga and YARP 3

I am experiencing a strange behavior with the following combination of software:

Project	Version
yarp	3.0.0
wb-toolbox	latest devel + yarp3 fixes (diegoferigo/WB-Toolbox@4ee6e10)
icub-main	latest devel (robotology/icub-main@4f5881a)
idyntree	latest master (robotology/idyntree@94cee9d)
gazebo-yarp-plugins	latest devel (robotology/gazebo-yarp-plugins@3170721)
codyco-modules	latest devel (robotology-legacy/codyco-modules@e1965bb)

I am pretty sure that with the latest devel of wb-toolbox, that compiles against yarp devel pre-3 release, the controller was working fine. I remember I tested the generated yoga controller.

The new behavior is the following:

I doubt this is a problem of the controller itself. More probable the issue is somewhere else, hard to tell at the moment.

cc @gabrielenava @traversaro @DanielePucci

Shall we add the harmonic drives inertia in the motors reflected inertia

What do you think on adding the inertia of the harmonic drives?
From the datasheet I saw their inertia and is about $0.054*10^{-4} kg m^2$ (this is the mass moment of inertia of the gearing with respect to the input)
The dimension are not so different from the motors inertia

Porting and cleaning up mex-wholeBodyModel

Library is deprecated. iDyntree wrappers will be used instead. mex-wholebodymodel will be put in legacy.

Use QP block from WBToolbox in the controllers

It is now available a QP block inside WBToolbox (devel). This block should substitute the current call through S-Function to the QPOASES algorithm that is done inside all Simulink controllers.

Rename the repo?

The custom names of repository in the robotology organization are all lowercase, and with dash separating the names.

Include waist IMU

Now the only IMU that can be used in the models is the iCub's head IMU. There is the possibility of changing the IMU name and therefore use different IMUs, e.g. the iCub's waist IMU, but it must be verified that there are no hard-coded calculations related to the head IMU (e.g. the neck angle correction).

Get YARP_ROBOT_NAME from the system instead of overwriting it

At the moment, the environmental variable YARP_ROBOT_NAME is overwritten in the Simulink initialization file.

This issue is for investigating the possibility of retrieving the YARP_ROBOT_NAME from the system by means of the matlab function getenv.

Update hard-coded coupling matrices and reduction ratio

Following the possible errors pointed out in robotology/robots-configuration#76, it is necessary to update the coupling matrices and the reduction ratio used in the controllers for computing the motors reflected inertia.

Document the naming convention for the variables saved in the Matlab workspace

In the new devel branch, I've changed the visualization part in the Simulink models and therefore also the variables that are saved in the Matlab workspace after stopping the model. This issue is to document the new naming convention.

Wrong torso coupling matrix

The coupling matrix for the torso that is used in the whole-body-controllers seems not compatible with the joints order configuration.

Given the motor-joints order used in the configuration:

index	joints	motors
0	torso_pitch	OB3M1
1	torso_roll	OB3M0
2	torso_yaw	OB4M0

The transform used in whole-body-controllers is:
$$ \begin{pmatrix} \text{torso_pitch} \\ \text{torso_roll} \\ \text{torso_yaw} \end{pmatrix} = \begin{pmatrix} 0.0 & -0.5 & 0.5 \\ 0.0 & 0.5 & 0.5 \\ 0.55 & 0.275 & 0.275 \end{pmatrix} \begin{pmatrix} \text{OB3M1} \\ \text{OB3M0} \\ \text{OB4M0} \end{pmatrix} $$

While, accordingly to the wiki and respecting the joints convention used in the configuration of the whole-body-controllers, it should be:
$$ \begin{pmatrix} \text{torso_pitch} \\ \text{torso_roll} \\ \text{torso_yaw} \end{pmatrix} = \begin{pmatrix} 0.5 & -0.5 & 0.0\\ 0.5 & 0.5 & 0.0 \\ 0.275 & 0.275 & 0.55 \end{pmatrix} \begin{pmatrix} \text{OB3M1} \\ \text{OB3M0} \\ \text{OB4M0} \end{pmatrix} $$


Probably, the error was due to the fact that the matrix was defined starting from the wiki and changing the row of the matrix without considering the motors order.

This impacts the computation of the motor reflected inertia

Documentation update for release 2.5

By adding notes and advices.

cc @aikolina

New features and legacy

This issue is to document where to find new features and legacy code associated to Matlab/Simulink whole body control.

Main repository: whole-body-controllers

Available controllers

• impedance-control
• momentum-based-standup
• momentum-based-yoga
• utilities

Matlab functions library

• matlab library

Active Forks (new features)

• force-parametrization
• PhRI-standup
• exploit friction and walking controller

Legacy

• seesaw controller and integration-based-ikin
• automatic gain tuning
• elastic joints control
• Walkman control: https://github.com/gabrielenava/mex-wholebodymodel/tree/master/controllers/torqueBalancingWalkman and https://github.com/gabrielenava/WBI-Toolbox-controllers/tree/walkman
• joint-space control and centroidal transformation

Associated repositories

• idyntree-high-level-wrappers

Check if the Simulink required integration step is respected

In view of robotology/wb-toolbox#160, it seems that there might be an issue with the Simulink gui that impairs the achievement of the desired integration step.

As first thing, we need to verify if there are differences between running the standard demos (Yoga, standup) from command line or from the Simulink gui.

@FabioBergonti if you already implemented a piece of code that allow to verify if the Simulink integration time is respected, can you please add it to this issue? I will then proceed to integrate it inside all the models.

Support dynamics integration inside Simulink

In the devel branch, I've modified all the Simulink controllers to isolate all the input/outputs from/to the Gazebo simulator:

This new configuration will be one day exploited for allowing the user to choose between performing the simulation with Gazebo or instead interface the controller's I/O using a forward dynamics block that will integrate the dynamics inside Simulink without the need of Gazebo.

Porting and cleaning up WBIToolbox-Controllers

Controllers to be ported: (see https://github.com/robotology-playground/WBI-Toolbox-controllers/tree/master/controllers)

• torqueBalancingYoga
• boundedTorqueControl
• torqueBalancingOnSeesaw
• torqueBalancingStandUp
• wholeBodyImpedance
• utilites
• tests on the real robots (iCubGenova02 and iCubGenova04)

Set control parameters from Simulink block

Currently, the control parameters such as gains and regularization terms are stored in matlab functions like this one.

The autogenerated C++ code from Simulink ( https://github.com/robotology-playground/autogenerated-whole-body-controllers) will have hard-coded gains that cannot be modified without editing the source code.

Therefore it may be useful to change the way Simulink loads these parameters, namely there should be a Simulink block that reads these values from a configuration file, that can be read both from the Simulink code and from the autogenerated c++ code.

cc @diegoferigo