[WIP] WBC release 2.5 (#77)

* added data processing utility * documentation of the repo updated * Update README.md * Update README.md * cleanup of documentation, cmake and models * added Simulink balancing simulator * added home positions for the yarpmotorgui * updated installation path of homePos * Revert "updated installation path of homePos" This reverts commit 55928fb. * fixed warning in CMakeLists.txt * added goToWholeBodyControllers.m generation * updated documentation for goToWholeBodyControllers.m * updated GUI (#82) and print warning when joints have spikes or exit range (#81) * added pictures of the simulink models * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update How-to-run-controllers-with-gazebo-simulator.md * Update How-to-run-controllers-with-gazebo-simulator.md * Update How-to-run-controllers-with-gazebo-simulator.md * Update How-to-setup-iCub-for-wbc-experiments.md * Update autogeneration (#92) * Add driver.cpp.in template * Update clang-format * Update main CMakeLists.txt to use the driver.cpp.in template * Minor edit in RegisteMdl.cmake * Update CreateAutogeneratedCodeTargets.cmake.in template * fix block names for autogen * fixed also the model without Gazebo for autogen * modified installation path of +wbc library * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * Update README.md * models exported to 2017b Co-authored-by: Diego Ferigo <[email protected]> Co-authored-by: Diego Ferigo <[email protected]>
robotology · Mar 23, 2020 · 8dd494b · 8dd494b
1 parent 2c9b626
commit 8dd494b
Show file tree

Hide file tree

Showing 198 changed files with 36,988 additions and 2,082 deletions.
diff --git a/.clang-format b/.clang-format
@@ -64,6 +64,10 @@ IncludeCategories:
     Priority:        2
   - Regex:           '^(<|"(gtest|gmock|isl|json)/)'
     Priority:        3
+  - Regex:           '^"rtwtypes'
+    Priority:        -1
+  - Regex:           '^"tmwtypes'
+    Priority:        -1
   - Regex:           '.*'
     Priority:        1
 IncludeIsMainRegex: '(Test)?$'
@@ -89,7 +93,7 @@ PenaltyExcessCharacter: 1000000
 PenaltyReturnTypeOnItsOwnLine: 60
 PointerAlignment: Left
 ReflowComments:  true
-SortIncludes:    false
+SortIncludes:    true
 SortUsingDeclarations: true
 SpaceAfterCStyleCast: true
 SpaceAfterTemplateKeyword: true

diff --git a/.gitignore b/.gitignore
@@ -31,7 +31,8 @@ slprj/
 *.mat
 
 # Matlab figures
-*.fig
+*.fig 
+!/library/matlab-gui/simulinkStaticGUI.fig
 
 # Simulink autosave
 *.autosave

diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,10 +5,26 @@ cmake_minimum_required(VERSION 3.5)
 project(whole-body-controllers)
 
 option(WBC_EXPORT_AUTOGENERATED "Enable the target to export code generated with Simulink Coder" OFF)
+option(WBC_INSTALL_ALL_HOME_POS "Installation of all available home positions" ON)
+
+# ======================================
+# Install home positions and WBC library 
+# ======================================
+
+find_package(YARP REQUIRED)
+yarp_configure_external_installation(wbc)
+
+add_subdirectory(utilities)
+add_subdirectory(library)
+add_subdirectory(config)
+
+# ======================
+# Autogeneration routine 
+# ======================
 
 if(NOT WBC_EXPORT_AUTOGENERATED)
     # Fake installation. 
-    install(CODE "MESSAGE(\"This repository simply acts as a container\")")
+    install(CODE "MESSAGE(\"Disabled exporting of autogenerated c++ code from Simulink.\")")
     return()
 endif()
 
@@ -37,8 +53,7 @@ file(WRITE "${CMAKE_BINARY_DIR}/autogenerated/CMakeLists.txt" "# Exported contro
 # Find clang-format
 find_program(ClangFormat_EXECUTABLE
     DOC "Path to the clang-format executable."
-    NAMES clang-format clang-format-7.0 clang-format-6.0 clang-format-5.0 clang-format-4.0
-    )
+    NAMES clang-format clang-format-9 clang-format-8 clang-format-7 clang-format-6.0 clang-format-5.0 clang-format-4.0)
 configure_file(${CMAKE_SOURCE_DIR}/.clang-format ${CMAKE_BINARY_DIR}/.clang-format COPYONLY)
 
 # Copy only h and cpp files
@@ -48,17 +63,21 @@ foreach(SOURCEDIR ${AUTOGENERATED_FOLDERS})
     file(APPEND "${CMAKE_BINARY_DIR}/autogenerated/CMakeLists.txt" "add_subdirectory(${MDLDIRNAME})\n")
     file(MAKE_DIRECTORY "${MDLDESTDIR}")
     string(REGEX MATCH "[^_]*" MDL_NAME ${MDLDIRNAME}) # Remove _gtw_rtw
+    # Create the CMakeLists.txt of the model
     configure_file(
         "${CMAKE_SOURCE_DIR}/cmake/CreateAutogeneratedCodeTargets.cmake.in"
         "${MDLDESTDIR}/CMakeLists.txt"
-        @ONLY
-        )
+        @ONLY)
+    # Copy the main file
+    configure_file(
+        "${PROJECT_SOURCE_DIR}/src/driver.cpp.in"
+        "${MDLDESTDIR}/driver.cpp"
+        @ONLY)
     # Copy sources to the build directory
     add_custom_command(TARGET copy-autogenerated-models
         PRE_BUILD
         COMMAND ${CMAKE_COMMAND} -E copy "${SOURCEDIR}/*.h" "${SOURCEDIR}/*.cpp" "${SOURCEDIR}/defines.txt" "${MDLDESTDIR}"
-        COMMENT "Copying ${MDLDIRNAME} to the build folder."
-        )
+        COMMENT "Copying ${MDLDIRNAME} to the build folder.")
     # Apply clang-format style
     if(NOT ${ClangFormat_EXECUTABLE} STREQUAL "ClangFormat_EXECUTABLE-NOTFOUND")
         add_custom_command(TARGET copy-autogenerated-models
@@ -72,5 +91,4 @@ endforeach()
 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"
-    )
+    COMMENT "Copying generated files to ${AUTOGENERATED_WBC_SOURCE_DIR}/autogenerated")
diff --git a/README.md b/README.md
@@ -1,31 +1,50 @@
 # whole-body-controllers
 
-The repository contains Simulink-based whole-body controllers for humanoid robots, and configuration and utility Matlab functions to perform balancing simulations with Gazebo simulator and on the Yarp-based robot [iCub](http://www.icub.org/). 
-
-**Warning! This repository 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:**
+**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.0](https://github.com/robotology/whole-body-controllers/releases/tag/v2.0)
 - [WBC v1.5](https://github.com/robotology/whole-body-controllers/releases/tag/v1.5)
 - [WBC v1.0](https://github.com/robotology/whole-body-controllers/releases/tag/v1.0)
 
+## Overview
+
+The repository contains `Simulink-based whole-body controllers` developed to control the [iCub](http://www.icub.org/) 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](https://github.com/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](library/README.md) of configuration and utility Matlab functions to design simulations with [Gazebo](http://gazebosim.org/) simulator and on the real robot iCub. 
+
+- The robot dynamics and kinematics is computed run-time by means of [WBToolbox](https://github.com/robotology/wb-toolbox), a Simulink library that wraps [iDyntree](https://github.com/robotology/idyntree). For more information on iDyntree library, see also this [README](https://github.com/robotology/idyntree/blob/master/README.md). 
+
+- 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](controllers/README.md) folder for more details.
+
 ## Dependencies
 
 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) 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)
+- [CMake](https://cmake.org/), at least version **3.5**.
+- [Matlab/Simulink](https://it.mathworks.com/products/matlab.html), default version **R2017b**.
+- [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), [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).
+- [whole-body-estimators](https://github.com/robotology/whole-body-estimators) (**Optional**, for using [wholeBodyDynamics](https://github.com/robotology/whole-body-estimators/tree/master/devices/wholeBodyDynamics) device).
+- [YARP](https://github.com/robotology/yarp) and [icub-main](https://github.com/robotology/icub-main).
 
 ## Installation and usage
 
-**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).
+The repository is usually tested and developed on **Ubuntu** and **macOS** operating systems. Some functionalities may not work properly on **Windows**.
 
-- 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.
+- **NOTE:** 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`, `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).
 
-- 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:
+- 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 |
   |:-------------:|:-------------:|
@@ -34,23 +53,23 @@ This repository depends upon the following Software:
   | 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: 
-   - 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. 
+- **IMPORTANT!** to use the WBC Simulink controllers, it is **required** to add the **installed** [+wbc](library/matlab-wbc/+wbc) folder to the Matlab path. There are two possible ways to add the folder to the Matlab path:
+
+   **1a.** `manually` and `permanently` add the folder to the Matlab path;
 
-   `[outputs] = wbc.myFunction(inputs)`. 
+   **1b.** run **only once** the [startup_WBC.m](config/startup_WBC.m.in) 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](config/createGoToWBC.m) in that folder. Run it to jump to the WBC folder. For further information on the installation procedure see also the [WBToolbox documentation](https://robotology.github.io/wb-toolbox/mkdocs/install/#matlab).
+    **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](https://github.com/robotology/robotology-superbuild/blob/master/cmake/template/startup_robotology_superbuild.m.in) file that comes along with robotology-superbuild installation.
+   - **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 on packages 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).
+- There are some functionalities of the repo such as the [automatic generation of c++ code from Simulink](https://github.com/robotology/whole-body-controllers#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](https://robotology.github.io/wb-toolbox/mkdocs/troubleshooting/).
 
-## Structure of the repo
+## Relevant folders of the repo
 
-- **config**: a collection of scripts for correctly configure this repo. [[README]](config/README.md)
+- **config**: a collection of scripts to correctly configure this repo. [[README]](config/README.md)
 
 - **controllers**: Simulink whole-body position and torque controllers for balancing of humanoid robots. [[README]](controllers/README.md)
 
@@ -62,9 +81,10 @@ Please refer to the [WBToolbox troubleshooting documentation](https://robotology
 
 ### Available controllers 
 
-- [fixed-base-joints-control](controllers/fixed-base-joints-control/README.md)
+- [fixed-base-joints-torque-control](controllers/fixed-base-joints-torque-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)
+- [simulink-balancing-simulator](controllers/simulink-balancing-simulator/README.md)
 
 ### Matlab functions library
 
@@ -80,16 +100,22 @@ There is the possibility to generate c++ code from the Simulink models using [Si
 
 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.
 
+### Home positions for yarpmotorgui
+
+The repo contains a set of predefined [home positions](utilities/homePositions) to be used with the [yarpmotorgui](https://www.yarp.it/yarpmotorgui.html). 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](utilities/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](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-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)
+- [exploit friction controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/momentum-based-yoga-friction)
+- [walking controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/task-based-walking)
+- [seesaw controller](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/torqueBalancingOnSeesaw)
+- [automatic gain tuning](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/torqueBalancingTuning) and [automatic gain tuning-matlab](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-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)
+- [walkman control](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/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)
+- [stand-up control 4 contacts](https://github.com/robotology-legacy/WBI-Toolbox-controllers/tree/master/controllers/torqueBalancingStandup_4Contacts)
 
 ## Citing this work
 

diff --git a/cmake/CreateAutogeneratedCodeTargets.cmake.in b/cmake/CreateAutogeneratedCodeTargets.cmake.in
@@ -1,22 +1,29 @@
+# Copyright (C) 2019 Istituto Italiano di Tecnologia (IIT). All rights reserved.
+# This software may be modified and distributed under the terms of the
+# GNU Lesser General Public License v2.1 or any later version.
+
+find_package(BlockFactory COMPONENTS SimulinkCoder REQUIRED)
+
+# ===============================================
 # Generate a library from the autogenerated class
 # ===============================================
 
-set(MDL_NAME @MDL_NAME@)
-add_generated_code_lib(MODELNAME ${MDL_NAME})
+include(TargetFromGeneratedCode)
+set(SIMULINK_MODEL_NAME "@MDL_NAME@")
 
+# This macro creates a target named as the mdl file for the autogenerated class
+target_from_generated_code(
+    MODELNAME ${SIMULINK_MODEL_NAME}
+    SOURCE_FOLDER ${CMAKE_CURRENT_SOURCE_DIR})
+
+# ================================
 # Test executable with custom main
 # ================================
 
 # Set the target name for the custom driver
-set(CODER_MAIN "CoderMain_${MDL_NAME}")
-
-configure_file(
-    "${PROJECT_SOURCE_DIR}/src/driver.cpp.in"
-    "${CMAKE_CURRENT_BINARY_DIR}/driver.cpp"
-    @ONLY)
+set(CODER_MAIN "Run${SIMULINK_MODEL_NAME}")
 
 add_executable(${CODER_MAIN}
-    "${CMAKE_CURRENT_BINARY_DIR}/driver.cpp"
-)
+    "${CMAKE_CURRENT_SOURCE_DIR}/driver.cpp")
 
-target_link_libraries(${CODER_MAIN} PUBLIC ${MDL_NAME}_LIB)
+target_link_libraries(${CODER_MAIN} PUBLIC ${SIMULINK_MODEL_NAME})