Porting to WBToolbox v.5.0 (#63)

* cleanup

* migration to wb5.0

* new yoga controller under testing

* almost working yoga demo

* working yoga in simulation

* added compatibility with matlab 2017b

* updated readme

* bugfix after testing on iCubGenova04

* models ported to WBT5

* updated simulink GUI

.gitattributes

@@ -2,4 +2,3 @@
 *.mdl -crlf -diff -merge
 *.mat -crlf -diff -merge
 *.mlx -crlf -diff -merge
-

.gitignore

@@ -43,4 +43,3 @@ slprj/
 ##---------------------------------------------------
 *.sublime-project
 *.sublime-workspace
-

CMakeLists.txt

@@ -18,7 +18,7 @@ list(APPEND CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake")
 include(RegisterMdl)
 initialize_mdl_set(NAME WHOLE_BODY_CONTROLLERS)
 
-add_subdirectory(torque-controllers)
+add_subdirectory(controllers)
 
 # ============================
 # EXPORT AUTOGENERATED SOURCES
@@ -73,4 +73,4 @@ add_custom_command(TARGET copy-autogenerated-models
     POST_BUILD
     COMMAND ${CMAKE_COMMAND} -E copy_directory "${CMAKE_BINARY_DIR}/autogenerated" "${AUTOGENERATED_WBC_SOURCE_DIR}/autogenerated"
     COMMENT "Copying generated files to ${AUTOGENERATED_WBC_SOURCE_DIR}/autogenerated"
-    )
+    )

README.md

@@ -1,38 +1,42 @@
 # whole-body-controllers
 
-Simulink-based whole body controllers for humanoid robots. 
+Simulink-based whole-body controllers for humanoid robots. 
 
 ## Dependencies
 
-This repository depends upon the following Software/repositories:
+This repository depends upon the following Software:
 
 - [Matlab/Simulink](https://it.mathworks.com/products/matlab.html), default version **R2017b**
-- [WB-Toolbox](https://github.com/robotology/WB-Toolbox)
+- [WB-Toolbox](https://github.com/robotology/WB-Toolbox) and [blockfactory](https://github.com/robotology/blockfactory)
 - [Gazebo Simulator](http://gazebosim.org/), default version **9.0**
 - [gazebo-yarp-plugins](https://github.com/robotology/gazebo-yarp-plugins)
-- [icub-gazebo](https://github.com/robotology/icub-gazebo) and [icub-gazebo-wholebody](https://github.com/robotology-playground/icub-gazebo-wholebody) to access iCub models.
+- [icub-gazebo](https://github.com/robotology/icub-gazebo), [icub-gazebo-wholebody](https://github.com/robotology-playground/icub-gazebo-wholebody) and [icub-models](https://github.com/robotology/icub-models) to access iCub models.
 - [codyco-modules](https://github.com/robotology/codyco-modules) (Optional, for using [home positions](https://github.com/robotology/codyco-modules/tree/master/src/modules/torqueBalancing/app/robots) and [wholeBodyDynamics](https://github.com/robotology/codyco-modules/tree/master/src/devices/wholeBodyDynamics) device).
 
-**NOTE:** it is suggested to install `whole-body-controllers` and most of its dependencies (namely, `codyco-modules`,`icub-gazebo`,`icub-gazebo-wholebody`, `gazebo-yarp-plugins` and `WB-Toolbox` and their dependencies) using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild) (enable `ROBOTOLOGY_USES_GAZEBO`, `ROBOTOLOGY_ENABLE_DYNAMICS`, `ROBOTOLOGY_USES_MATLAB` options).
-
 ## Installation and usage
 
-- clone the repository on your pc by running on a terminal `git clone https://github.com/robotology/whole-body-controllers`, or dowload the repository.
+**NOTE:** it is suggested to install `whole-body-controllers` and most of its dependencies (namely, `codyco-modules`,`icub-gazebo`,`icub-gazebo-wholebody`, `icub-models`, `gazebo-yarp-plugins`, `blockfactory` and `WB-Toolbox` and their dependencies) using the [robotology-superbuild](https://github.com/robotology/robotology-superbuild) (enable `ROBOTOLOGY_USES_GAZEBO`, `ROBOTOLOGY_ENABLE_DYNAMICS`, `ROBOTOLOGY_USES_MATLAB` options).
+
+- Otherwise, clone the repository on your pc by running on a terminal `git clone https://github.com/robotology/whole-body-controllers`, or dowload the repository.
 
 - set the environmental variable `YARP_ROBOT_NAME` in the `.bashrc` file to be the name of the robot you want to control. List of supported robot names:
 
   | Robot Names | Associated URDF Model |
   |:-------------:|:-------------:|
   | iCubGenova02 | [model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGenova02/model.urdf) |
   | iCubGenova04 | [model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGenova04/model.urdf) |
-  | iCubGazeboV2_5 |[model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGazeboV2_5/model.urdf)|
+  | iCubGazeboV2_5|[model.urdf](https://github.com/robotology/icub-models/blob/master/iCub/robots/iCubGazeboV2_5/model.urdf)|
   | icubGazeboSim |[model.urdf](https://github.com/robotology/yarp-wholebodyinterface/blob/master/app/robots/icubGazeboSim/model.urdf) |
 
-- to use the Simulink controllers, it is required to add the [matlab-wbc](library/matlab-wbc) folder to the Matlab path. There are two different possible ways to add the folder to the path: 
-   - directly add the folder to the Matlab path;
-   - run **only once** the [startup.m](config/startup.m) script. In this case, it is required to **always** start matlab from the folder where the `pathdef.m` file is (usually `~/Documents/MATLAB`). For further information see also the [WBToolbox documentation](https://robotology.github.io/wb-toolbox/mkdocs/install/#matlab).
+- to use the Simulink controllers, it is **required** to add the [matlab-wbc](library/matlab-wbc) folder to the Matlab path. There are two different possible ways to add the folder to the path: 
+   - manually add the folder to the Matlab path;
+   - run **only once** the [startup_WBC.m](config/startup_WBC.m) script. In this case, it is required to **always** start matlab from the folder where the `pathdef.m` file is (usually `~/Documents/MATLAB`). For further information see also the [WBToolbox documentation](https://robotology.github.io/wb-toolbox/mkdocs/install/#matlab).
+
+   **Note**: to use any function inside the package [matlab-wbc/+wbc](library/matlab-wbc/+wbc), add the `wbc` prefix to the function name when the function is invoked, i.e. 
+
+   `[outputs] = wbc.myFunction(inputs)`. 
 
-   **Note**: to use any function inside the package [matlab-wbc/+wbc](library/matlab-wbc/+wbc), add the `wbc` prefix to the function name when the function is invoked, i.e. `[outputs] = wbc.myFunction(inputs)`. More information can be found in the [Matlab documentation](https://it.mathworks.com/help/matlab/matlab_oop/scoping-classes-with-packages.html).
+   More information on packages can be found in the [Matlab documentation](https://it.mathworks.com/help/matlab/matlab_oop/scoping-classes-with-packages.html).
 
 ## Troubleshooting
 
@@ -42,57 +46,71 @@ Please refer to the [WBToolbox troubleshooting documentation](https://robotology
 
 - **config**: a collection of scripts for correctly configure this repo. [[README]](config/README.md)
 
-- **torque-controllers**: Simulink torque controllers for balancing and walking of humanoid robots. [[README]](torque-controllers/README.md)
+- **controllers**: Simulink whole-body position and torque controllers for balancing of humanoid robots. [[README]](controllers/README.md)
 
 - **doc**: guidelines on how to create/use Simulink models for control. [[README]](doc/README.md)
 
 - **library**: a library of functions/scripts used by the controllers. [[README]](library/README.md)
 
 - **utilities**: Simulink models for debugging sensors on the real robot. [[README]](utilities/README.md)
 
-## Interesting features
-
-### Automatic generation of c++ code from Simulink
-
-There is the possibility to generate c++ code from the Simulink models using [Simulink coder](https://www.mathworks.com/products/simulink-coder.html) (**available only for the [momentum-based-yoga](torque-controllers/momentum-based-yoga)**). The repositiory that contains the generated c++ code is named [autogenerated-whole-body-controllers](https://github.com/robotology-playground/autogenerated-whole-body-controllers). Documentation on how to generate the code is available in the repository [wiki](https://github.com/robotology-playground/autogenerated-whole-body-controllers/wiki/How-to-generate-code-from-a-Simulink-model).
+### Available controllers 
 
-### 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](https://github.com/robotology/wb-toolbox/issues/160). 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](library/matlab-gui) has been developed. **Available only for [momentum-based-yoga](torque-controllers/momentum-based-yoga) and [momentum-based-standup](torque-controllers/momentum-based-standup)**. If you want to run Simulink with the static GUI, just run the [start{$SIM_NAME}WithoutSimulinkGui](torque-controllers/momentum-based-yoga/startYogaWithoutSimulinkGui.m) script.
+- [fixed-base-joints-control](controllers/fixed-base-joints-control/README.md)
+- [floating-base-balancing-position-control](controllers/floating-base-balancing-position-control/README.md)
+- [floating-base-balancing-torque-control](controllers/floating-base-balancing-torque-control/README.md)
 
-## Where do I find new features and legacy repos?
+### Matlab functions library
 
-#### Available controllers 
+- [matlab WBC library](library/matlab-wbc)
 
-- [impedance-control](https://github.com/robotology/whole-body-controllers/tree/master/torque-controllers/impedance-control)
-- [momentum-based-standup](https://github.com/robotology/whole-body-controllers/tree/master/torque-controllers/momentum-based-standup)
-- [momentum-based-yoga](https://github.com/robotology/whole-body-controllers/tree/master/torque-controllers/momentum-based-yoga)
-- [utilities](https://github.com/robotology/whole-body-controllers/tree/master/utilities)
+## Additional features
 
-#### Matlab functions library
+### Automatic generation of c++ code from Simulink
 
-- [matlab library](library/matlab-wbc)
+There is the possibility to generate c++ code from the Simulink models using [Simulink coder](https://www.mathworks.com/products/simulink-coder.html) (**available only for the [floating-base-balancing-torque-control](controllers/floating-base-balancing-torque-control)**). The repositiory that contains the generated c++ code is named [autogenerated-whole-body-controllers](https://github.com/robotology-playground/autogenerated-whole-body-controllers). Documentation on how to generate the code is available in the repository [wiki](https://github.com/robotology-playground/autogenerated-whole-body-controllers/wiki/How-to-generate-code-from-a-Simulink-model).
 
-#### Active Forks (new features)
+### Static GUI for Simulink
 
-- [force-parametrization](https://github.com/ahmadgazar/whole-body-controllers)
-- [PhRI-standup](https://github.com/Yeshasvitvs/wholeBodyControllers)
+When used for controlling real platforms, heavy Simulink models may violate the user-defined simulation time step, see also [this issue](https://github.com/robotology/wb-toolbox/issues/160). 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](library/matlab-gui) has been developed. If you want to run Simulink with the static GUI, run the [startModelWithStaticGui](controllers/floating-base-balancing-torque-control/startModelWithStaticGui.m) script.
 
-#### Legacy
+## Where do I find legacy materials?
 
-Official legacy repositories are: [mex-wholebodymodel](https://github.com/robotology/mex-wholebodymodel) and [WBI-Toolbox-controllers](https://github.com/robotology-playground/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.
+Official legacy repositories are: [mex-wholebodymodel](https://github.com/robotology/mex-wholebodymodel) and [WBI-Toolbox-controllers](https://github.com/robotology-legacy/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 and walking controller](https://github.com/robotology-playground/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy)
-- [seesaw controller and integration-based-ikin](https://github.com/robotology-playground/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy)
+- [exploit friction and walking controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy)
+- [seesaw controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy)
 - [automatic gain tuning](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingGainTuning)
 - [elastic joints control](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancing_JE)
-- [walkman control](https://github.com/robotology-playground/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy/torqueBalancing-walkman) and [walkman control-matlab](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingWalkman)
+- [walkman control](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/whole-body-controllers-legacy/controllers/legacy/torqueBalancing-walkman) and [walkman control-matlab](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingWalkman)
 - [joint-space control and centroidal transformation](https://github.com/robotology/mex-wholebodymodel/tree/master/controllers/torqueBalancingJointControl)
 
-#### Associated repositories
-
-- [matlab-multi-body-sim](https://github.com/gabrielenava/matlab-multi-body-sim)
-
-### Mantainers
-
-Gabriele Nava ([@gabrielenava](https://github.com/gabrielenava))
+## 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
+
+Gabriele Nava ([@gabrielenava](https://github.com/gabrielenava))

cmake/CreateAutogeneratedCodeTargets.cmake.in

@@ -19,4 +19,4 @@ add_executable(${CODER_MAIN}
     "${CMAKE_CURRENT_BINARY_DIR}/driver.cpp"
 )
 
-target_link_libraries(${CODER_MAIN} PUBLIC ${MDL_NAME}_LIB)
+target_link_libraries(${CODER_MAIN} PUBLIC ${MDL_NAME}_LIB)

cmake/RegisterMdl.cmake

@@ -25,7 +25,6 @@ function(initialize_mdl_set)
         FULL_DOCS "Array with the path of the folders to export containing autogenerated code.")
 endfunction()
 
-
 function(register_mdl)
 
     # ===============
@@ -63,4 +62,4 @@ function(register_mdl)
     # Register the mdl by appending its name to the global property
     set_property(GLOBAL APPEND PROPERTY ${SETNAME} ${AUTOGENSOURCES_ABSPATH})
 
-endfunction()
+endfunction()

config/README.md

@@ -1,12 +1,7 @@
 # Configuration scripts
 
-A collection of scripts used for correctly configure the repo.
-
-- [export_WBC.m](export_WBC.m): run this script once. Then, digit the Matlab version in which you want to export the Simulink models. All models in the repo will be exported in that version. By default, Simulnk models in this repo are written using Matlab 2017b. **Remember: you cannot export a model in a Matlab version newer than the one you are using**! 
-
-- [startup.m](startup.m): run this script once. Then, the path to the `matlab-wbc` folder will be permanently added to the `pathdef.m` file, which is saved inside the Matlab `userpath`. In order to have the `matlab-wbc` folder inside the Matlab path, it is required to start Matlab from the folder where the `pathdef.m` file is (in general, `~/Documents/MATLAB`).
-
-
-
+A collection of scripts used for configuring the repo.
 
+- [export_WBC.m](export_WBC.m): run this script. Then, digit the Matlab version in which you want to export the Simulink models. All models in the repo will be exported to that version. By default, Simulnk models in this repo are written using Matlab 2017b. **Remember: you cannot export a model in a Matlab version newer than the one you are using**! 
 
+- [startup_WBC.m](startup_WBC.m): run this script. Then, the path to the `matlab-wbc` folder will be **permanently** added to your `pathdef.m` file, which will be saved inside the Matlab `userpath`. In order to have the `matlab-wbc` folder inside the Matlab path, it is **required** to start Matlab from the folder where the `pathdef.m` file is (i.e., from the folder that the `userpath` is pointing, usually `~/Documents/MATLAB`).

config/startup.m → config/startup_WBC.m

@@ -1,9 +1,9 @@
-%% startup.m
+%% startup_WBC.m
 %
-%  Run this script only once to permanently add the matlab-wbc folder to your MATLAB path. 
+%  Run this script once to permanently add the matlab-wbc library to your MATLAB path. 
 
 fprintf('\n## whole-body-controllers ##\n');
-fprintf('\nAdding "matlab-wbc" to pathdef.m...\n\n');
+fprintf('\nAdding "matlab-wbc" library to pathdef.m...\n\n');
 
 % path to whole-body-controllers
 pathToWBC = pwd;

controllers/CMakeLists.txt

@@ -0,0 +1 @@
+add_subdirectory(floating-base-balancing-torque-control)

controllers/README.md

@@ -0,0 +1,13 @@
+# Controllers
+
+Simulink controllers for humanoid robots. 
+
+## List of available controllers:
+
+- **fixed-base-joints-control** [[README]](fixed-base-joints-control/README.md)
+- **floating-base-balancing-position-control** [[README]](floating-base-balancing-position-control/README.md)
+- **floating-base-balancing-torque-control** [[README]](floating-base-balancing-torque-control/README.md)
+
+## Usage
+
+See corresponding READMEs, and check the [documentation](https://github.com/robotology-playground/whole-body-controllers/tree/master/doc) of the repo.

controllers/fixed-base-joints-control/README.md

@@ -0,0 +1,21 @@
+## Module description
+
+This module implements a simple torque control balancing strategy. The robot is assumed to have its `base_link` fixed on a pole. Input torques are the `gravity toruqes` which allow to perform **gravity compensation**. Optionally, it is also possible to move each controlled joint in order to track a desired joints trajectory.
+
+### Compatibility
+
+The folder contains the Simulink model `jointsControl.mdl`, which is generated by using Matlab R2017b.
+
+### Supported robots
+
+Currently, supported robots are: `iCubGenova04`, `iCunGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`.
+
+## Module details
+
+### Configuration file
+
+At start, the module calls the initialization file `initJointsControl.m`. Once opened, this file contains some configuration variables. Please follow the instruction inside the script to properly configure your simulation.
+
+### Robot and demo specific configurations
+
+The gains and references for a specific robot (specified by the variable `YARP_ROBOT_NAME`) or a specific demo can be found in the folder `app/robots/YARP_ROBOT_NAME`.

controllers/fixed-base-joints-control/app/robots/iCubGazeboV2_5/configJointsControl.m

@@ -0,0 +1,13 @@
+% CONFIGJOINTSCONTROL configures all the options associated to the
+%                     joints controller.
+%
+
+%% --- Initialization ---
+
+% Default behaviour: gravity compensation. If Config.MOVE_JOINTS = true,
+% the robot will also move all actuated joints following a sine trajectory
+Config.MOVE_JOINTS = false;
+
+% Max unsigned difference between two consecutive (measured) joint positions, 
+% i.e. delta_jointPos = abs(jointPos(k) - jointPos(k-1)) [rad]
+Sat.maxJointsPositionDelta = 15*pi/180;