Merge pull request #126 from robotology/nunoguedelha-patch-1

- Update README (after #122)
- Converted floating-base-balancing-torque-control-with-simulator into a package +floatingBaseBalancingTorqueControlWithSimulator
- Added the package installation through CMake.

CMakeLists.txt

@@ -8,7 +8,7 @@ option(WBC_EXPORT_AUTOGENERATED "Enable the target to export code generated with
 option(WBC_INSTALL_ALL_HOME_POS "Installation of all available home positions" ON)
 
 # ======================================
-# Install home positions and WBC library 
+# Install home positions and WBC library
 # ======================================
 
 find_package(YARP REQUIRED)
@@ -18,12 +18,26 @@ add_subdirectory(utilities)
 add_subdirectory(library)
 add_subdirectory(config)
 
+# Install the example controller model (YOGA+matlab-simulator)
+set(FLOATING_BASE_BALANCING_TORQCTRL_INSTALL_MATLAB_FILESDIR ${CMAKE_INSTALL_PREFIX}/mex/+wbc/examples CACHE
+    STRING "Location (relative to the install prefix) in which the Matlab .m files and simulink .mdl models are installed.")
+set(M_FILES_DIR ${whole-body-controllers_SOURCE_DIR}/controllers/+floatingBaseBalancingTorqueControlWithSimulator)
+
+install(
+  DIRECTORY ${M_FILES_DIR}
+  DESTINATION ${FLOATING_BASE_BALANCING_TORQCTRL_INSTALL_MATLAB_FILESDIR}
+  USE_SOURCE_PERMISSIONS
+  FILES_MATCHING
+  PATTERN "*.m"
+  PATTERN "*.jpeg"
+  PATTERN "*.mdl")
+
 # ======================
-# Autogeneration routine 
+# Autogeneration routine
 # ======================
 
 if(NOT WBC_EXPORT_AUTOGENERATED)
-    # Fake installation. 
+    # Fake installation.
     install(CODE "MESSAGE(\"Disabled exporting of autogenerated c++ code from Simulink.\")")
     return()
 endif()

README.md

@@ -11,7 +11,7 @@
 
 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 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. With the dependency [matlab-whole-body-simulator](https://github.com/dic-iit/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](controllers/floating-base-balancing-torque-control-with-simulator/README.md)).
 
 - 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). 
 
@@ -24,6 +24,7 @@ This repository depends upon the following Software:
 - [CMake](https://cmake.org/), at least version **3.5**.
 - [Matlab/Simulink](https://it.mathworks.com/products/matlab.html), default version **R2019b**.
 - [WB-Toolbox](https://github.com/robotology/WB-Toolbox) and [blockfactory](https://github.com/robotology/blockfactory).
+- [matlab-whole-body-simulator](https://github.com/dic-iit/matlab-whole-body-simulator), at least version **2.0.0**.
 - [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.
@@ -34,7 +35,7 @@ This repository depends upon the following Software:
 
 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`, `blockfactory` and `WB-Toolbox` and their dependencies) using the [robotology-superbuild](https://github.com/robotology/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](https://github.com/robotology/whole-body-controllers/blob/master/README.md#dependencies). Also, in the superbuild you have to enable the `ROBOTOLOGY_USES_GAZEBO` and `ROBOTOLOGY_USES_MATLAB` options.
+- 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](https://github.com/robotology/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](https://github.com/robotology/whole-body-controllers/blob/master/README.md#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:
 
@@ -54,14 +55,16 @@ The repository is usually tested and developed on **Ubuntu** and **macOS** opera
   | 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) |
 
-- **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:
+- **IMPORTANT!** to use the WBC Simulink controllers, it is **required** to add the **installed** `+wbc` and `+wbc/simulink` folders (copied from [+wbc](library/matlab-wbc/+wbc)) 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;
+   **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](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).
-
+
+- 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](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
@@ -85,8 +88,8 @@ Please refer to the [WBToolbox troubleshooting documentation](https://robotology
 - [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)
+- [floating-base-balancing-torque-control-with-simulator](controllers/floating-base-balancing-torque-control-with-simulator/README.md)
 - [floating-base-jerk-control](controllers/floating-base-jerk-control/README.md)
-- [simulink-balancing-simulator](controllers/simulink-balancing-simulator/README.md)
 
 ### Matlab functions library
 
@@ -119,6 +122,9 @@ Official legacy repositories are: [mex-wholebodymodel](https://github.com/roboto
 - [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)
 
+You can also find other legacy controllers/simultors in this repository **whole-body-controllers** in specific commits:
+- [simulink-balancing-simulator@c217f051](https://github.com/robotology/whole-body-controllers/tree/c217f051b16da32c8acc607182524239b3a7d8fb/controllers/simulink-balancing-simulator).
+
 ## 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:

controllers/+floatingBaseBalancingTorqueControlWithSimulator/README.md

@@ -0,0 +1,75 @@
+## Module description
+
+This module implements the same torque controller described in [floating-base-balancing-torque-control](../floating-base-balancing-torque-control/README.md), the block **"MOMENTUM BASED TORQUE CONTROL"**, but instead of controlling the real iCub robot or the model on Gazebo, it is integrated with the robot simulator from [matlab-whole-body-simulator](https://github.com/dic-iit/matlab-whole-body-simulator).
+
+<img src="/doc/pics/torqueControlwithSimu_w_SampleRates.png" width="1800">
+
+In the above diagram, signals and blocks sampling times are identified by colors
+- red: 1 ms
+- pink: constant
+- green: 10 ms
+
+More precisely:
+- The controller **"MOMENTUM BASED TORQUE CONTROL"** feeds the joint torques to the robot dynamics simulator **"Robot Simulator"** through a memory block for avoiding algebraic loops.
+- The **"Robot Simulator"** simulates the robot dynamics, handling the contacts of the feet with the ground. This sub-system provides the inputs required by the controller:
+    - joint positions
+    - joint velocities
+    - joint accelerations
+    - left foot wrench
+    - right foot wrench
+    - IMU measurements
+- The **"Robot Visualizer"** allows to visualize the robot. It avoids using Gazebo as a visualizer. As a result, the overall procedure for running the simulation is simpler (no need to run Yarp nor Gazebo, nor synchronize them with Matlab).
+
+<img src="/doc/pics/robotSimulator_w_SampleRates.png" width="1800">
+
+The **"Robot Simulator"** has its own **"Config"** block, for a robot configuration specific to the simulation, and is composed of three other blocks:
+- The **"RobotDynWithContacts"**, actual simulator core, simulates the robot dynamics and provides all the robot kinematic and dynamic quantities.
+- The **"IMU"** emulates the sensor outputs from the floating base state and linear acceleration.
+- The **"Friction Model"** block implements a simple viscous friction model, function of the joints velocities and desired torques.
+
+"RobotDynWithContacts", "IMU" and "RobotVisualizer" blocks are library blocks imported from [`matlab-whole-body-simulator`](https://github.com/dic-iit/matlab-whole-body-simulator). Their parameters are set through their respective masks. For further details on the "RobotDynWithContacts" configuration, refer to https://github.com/dic-iit/matlab-whole-body-simulator/blob/master/README.md.
+
+### Compatibility
+
+The folder contains the Simulink model `torqueControlBalancingWithSimu.mdl`, which is generated by using Matlab R2020b.
+
+### Supported robots
+
+Currently, the only supported robot model is `iCubGazeboV2_5` which has the proper inertia tuning (for the very small and light links within the shoulders, hips, etc).
+
+## Module details
+
+### How to run the demo
+
+You don't need any module external module running outside of Matlab.
+
+For running the simulation from the source module (for developpers wishing to perform any modification), follow the few steps below:
+1. Set the `YARP_ROBOT_NAME` environment variable to the desired model. The default and only currently supported model is `iCubGazeboV2_5`. There is no direct link with Gazebo. This model is suitable for the `matlab-whole-body-simulator` because of the modified inertia of the intermediate small and light links within the 3-DoF joints (shoulder pitch-roll-yaw, hip pitch-roll-yaw, etc), tuned for stabilising the dynamics of the simulation.
+    ```
+    > setenv(`YARP_ROBOT_NAME`,`iCubGazeboV2_5`)
+    ```
+
+2. Verify that the target robot model is available. You can check if the controller is targeting the correct robot model by typing on the Matlab command line:
+    ```
+    system('yarp resource --find model.urdf')
+    ```
+
+    then, check that the path and the model name are correct.
+
+3. Change the working directory to the [parent folder of the controller model](./).
+4. Open the Simulink model `torqueControlBalancingWithSimu.mdl`.
+5. Run the model.
+
+For running the simulation from the installed module (e.g. in case of a user, without any experience in simulation nor control, just trying the simulator for the first time), skip steps 3. and 4. and execute instead:
+- still in Matlab, and from any location except from [whole-body-controllers/controllers](../) (for avoiding file naming collision with [this folder](./)), open the model directly from the Matlab command line:
+```
+>> floatingBaseBalancingTorqueControlWithSimulator.torqueControlBalancingWithSimu
+```
+
+### Configuration file
+
+At start, the module calls the initialization file `initTorqueControlBalancingWithSimu.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/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m → controllers/+floatingBaseBalancingTorqueControlWithSimulator/initTorqueControlBalancingWithSimu.m

@@ -21,8 +21,11 @@
 clearvars -except sl_synch_handles simulinkStaticGUI
 clc
 
+% Get the parent folder of the running *.mdl model
+MODEL_PATH = fileparts(which(bdroot));
+
 % Add path to local source code
-addpath('./src/')
+addpath(strcat(MODEL_PATH,'/src/'));
 
 %% GENERAL SIMULATION INFO
 %
@@ -73,12 +76,14 @@
 switch(getenv('YARP_ROBOT_NAME'))
     case 'iCubGazeboV2_5'
     otherwise
-        error(['Unsupported robot model. Supported models are listed below:',repmat('\n- %s',[1 numel(supportedMmodels)])],supportedMmodels{:});
+        warning(['Unsupported robot model. Supported models are listed below:',repmat('\n- %s',[1 numel(supportedMmodels)])],supportedMmodels{:});
+        setenv('YARP_ROBOT_NAME','iCubGazeboV2_5');
+        warning('Setting the default model iCubGazeboV2_5.');
 end
-run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m'));
-run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configStateMachine.m'));
-run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m'));
-run(strcat('app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobotSim.m'));
+run(strcat(MODEL_PATH,'/app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobot.m'));
+run(strcat(MODEL_PATH,'/app/robots/',getenv('YARP_ROBOT_NAME'),'/configStateMachine.m'));
+run(strcat(MODEL_PATH,'/app/robots/',getenv('YARP_ROBOT_NAME'),'/gainsAndReferences.m'));
+run(strcat(MODEL_PATH,'/app/robots/',getenv('YARP_ROBOT_NAME'),'/configRobotSim.m'));
 run('initVisualizer');
 
 % Deactivate/activate the internal coordinator