diff --git a/CMakeLists.txt b/CMakeLists.txt
index ca82fdfe..0f1fefb3 100644
--- a/CMakeLists.txt
+++ b/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()
diff --git a/README.md b/README.md
index 193b3d6c..53539b51 100644
--- a/README.md
+++ b/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:
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/CMakeLists.txt
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/CMakeLists.txt
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/CMakeLists.txt
diff --git a/controllers/+floatingBaseBalancingTorqueControlWithSimulator/README.md b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/README.md
new file mode 100644
index 00000000..440e7225
--- /dev/null
+++ b/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).
+
+
+
+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).
+
+
+
+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`.
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobot.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configRobot.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobot.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configRobot.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configRobotSim.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configRobotSim.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configRobotSim.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configStateMachine.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/configStateMachine.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/configStateMachine.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/app/robots/iCubGazeboV2_5/gainsAndReferences.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/images/initNeckPosBlock.jpeg b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/images/initNeckPosBlock.jpeg
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/images/initNeckPosBlock.jpeg
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/images/initNeckPosBlock.jpeg
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/initTorqueControlBalancingWithSimu.m
similarity index 79%
rename from controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/initTorqueControlBalancingWithSimu.m
index 9ef27f7a..4e11c0cd 100644
--- a/controllers/floating-base-balancing-torque-control-with-simulator/initTorqueControlBalancingWithSimu.m
+++ b/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
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/closeModel.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/src-static-gui/closeModel.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/closeModel.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/src-static-gui/closeModel.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/compileModel.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/src-static-gui/compileModel.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/src-static-gui/compileModel.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/src-static-gui/compileModel.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/initVisualizer.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/src/initVisualizer.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/initVisualizer.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/momentumBasedController.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/momentumBasedController.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/src/momentumBasedController.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/momentumBasedController.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_oneFoot.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/processOutputQP_oneFoot.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_oneFoot.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/processOutputQP_oneFoot.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_twoFeet.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/processOutputQP_twoFeet.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/src/processOutputQP_twoFeet.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/processOutputQP_twoFeet.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/src/stateMachineMomentumControl.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/stateMachineMomentumControl.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/src/stateMachineMomentumControl.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/src/stateMachineMomentumControl.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/startModelWithStaticGui.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/startModelWithStaticGui.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/startModelWithStaticGui.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/startModelWithStaticGui.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancingWithSimu.m b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/stopTorqueControlBalancingWithSimu.m
similarity index 100%
rename from controllers/floating-base-balancing-torque-control-with-simulator/stopTorqueControlBalancingWithSimu.m
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/stopTorqueControlBalancingWithSimu.m
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/torqueControlBalancingWithSimu.mdl
similarity index 99%
rename from controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl
rename to controllers/+floatingBaseBalancingTorqueControlWithSimulator/torqueControlBalancingWithSimu.mdl
index 1b451127..34d5344a 100644
--- a/controllers/floating-base-balancing-torque-control-with-simulator/torqueControlBalancingWithSimu.mdl
+++ b/controllers/+floatingBaseBalancingTorqueControlWithSimulator/torqueControlBalancingWithSimu.mdl
@@ -7,7 +7,7 @@ Model {
NumRootInports 0
NumRootOutports 0
ParameterArgumentNames ""
- ComputedModelVersion "3.123"
+ ComputedModelVersion "3.131"
NumModelReferences 0
NumTestPointedSignals 0
NumProvidedFunctions 0
@@ -312,30 +312,30 @@ Model {
SID "4921"
Type "LIBRARY_BLOCK"
}
- ExternalFileReference {
- Reference "mwbs_visualizers_lib/RobotVisualizer"
- Path "torqueControlBalancingWithSimu/Robot Visualizer/RobotVisualizer"
- SID "4939"
- Type "LIBRARY_BLOCK"
- }
ExternalFileReference {
Reference "WBToolboxLibrary/Utilities/Configuration"
- Path "torqueControlBalancingWithSimu/Subsystem/Configuration"
+ Path "torqueControlBalancingWithSimu/Robot Simulator/Configuration"
SID "4962"
Type "LIBRARY_BLOCK"
}
ExternalFileReference {
Reference "mwbs_robotSensors_lib/IMUsensor"
- Path "torqueControlBalancingWithSimu/Subsystem/IMUsensor"
+ Path "torqueControlBalancingWithSimu/Robot Simulator/IMUsensor"
SID "4938"
Type "LIBRARY_BLOCK"
}
ExternalFileReference {
Reference "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts"
- Path "torqueControlBalancingWithSimu/Subsystem/RobotDynWithContacts"
+ Path "torqueControlBalancingWithSimu/Robot Simulator/RobotDynWithContacts"
SID "4937"
Type "LIBRARY_BLOCK"
}
+ ExternalFileReference {
+ Reference "mwbs_visualizers_lib/RobotVisualizer"
+ Path "torqueControlBalancingWithSimu/Robot Visualizer/RobotVisualizer"
+ SID "4939"
+ Type "LIBRARY_BLOCK"
+ }
OrderedModelArguments 1
}
SLCCPlugin "on"
@@ -357,21 +357,22 @@ Model {
FPTRunName "Run 1"
MaxMDLFileLineLength 120
CloseFcn "%% Try to close the GUI (the user might have already closed it)\ntry\n\n close(simulinkStaticGUI)\n\nend"
- InitFcn "%% initialize the simulation\ncd(fileparts(which(bdroot)));\ninitTorqueControlBalancingWithSimu;\n\n%% T"
- "ry to edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n set(sl_synch_handle"
- "s.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_synch_handles.startButton,'Enable','on');\n\nend"
+ InitFcn "%% initialize the simulation\nfloatingBaseBalancingTorqueControlWithSimulator.initTorqueControlBalancing"
+ "WithSimu;\n\n%% Try to edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n se"
+ "t(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n set(sl_synch_handles.startButton,'Enable','on'"
+ ");\n\nend"
StartFcn "%% Try to edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n se"
"t(sl_synch_handles.startButton,'Backgroundcolor',[0.8,0.8,0.8]);\n set(sl_synch_handles.startButton,'Enable','off"
"');\n\n %% Update Compile Button\n set(sl_synch_handles.compileButton,'Backgroundcolor',[0.8,0.8,0.8]);\n s"
"et(sl_synch_handles.compileButton,'Enable','off');\n\n %% Update Exit Button\n set(sl_synch_handles.exitButton"
",'Backgroundcolor',[0.8,0.8,0.8]);\n set(sl_synch_handles.exitButton,'Enable','off');\n\nend\n\n"
- StopFcn "%% stop the simulation\ncd(fileparts(which(bdroot)));\nstopTorqueControlBalancingWithSimu;\n\n%% Try to "
- "edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n if get(sl_synch_handles.c"
- "heckbox_recompile, 'Value') \n set(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,0.0]);\n s"
- "et(sl_synch_handles.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(sl_synch_handles.c"
- "ompileButton,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on');\n\n %% Upd"
- "ate Exit Button\n set(sl_synch_handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]);\n set(sl_synch_handles.exi"
- "tButton,'Enable','on');\n\nend\n\n\n"
+ StopFcn "%% stop the simulation\nfloatingBaseBalancingTorqueControlWithSimulator.stopTorqueControlBalancingWithSi"
+ "mu;\n\n%% Try to edit the GUI (the user may have already closed it)\ntry\n\n %% Update Start Button\n if get(s"
+ "l_synch_handles.checkbox_recompile, 'Value') \n set(sl_synch_handles.startButton,'Backgroundcolor',[0.0,1.0,"
+ "0.0]);\n set(sl_synch_handles.startButton,'Enable','on');\n end\n\n %% Update Compile Button\n set(s"
+ "l_synch_handles.compileButton,'Backgroundcolor',[1.0,0.6,0.0]);\n set(sl_synch_handles.compileButton,'Enable','on"
+ "');\n\n %% Update Exit Button\n set(sl_synch_handles.exitButton,'Backgroundcolor',[0.0,1.0,1.0]);\n set(sl_"
+ "synch_handles.exitButton,'Enable','on');\n\nend\n\n\n"
LastSavedArchitecture "maci64"
Object {
$PropName "BdWindowsInfo"
@@ -410,9 +411,9 @@ Model {
ViewObjType "SimulinkTopLevel"
LoadSaveID "0"
Extents [2171.0, 1188.0]
- ZoomFactor [2.0627701612843352]
- Offset [713.17598406081265, 157.57969597446328]
- SceneRectInView [713.17598406081265, 157.57969597446328, 1052.4682006493044, 575.92456120284373]
+ ZoomFactor [2.372878019401897]
+ Offset [714.07882907428, 185.18910568892798]
+ SceneRectInView [714.07882907428, 185.18910568892798, 914.922715052676, 500.65784683674764]
}
Object {
$ObjectID 6
@@ -422,14 +423,36 @@ Model {
LoadSaveID "4955"
Extents [2171.0, 1188.0]
ZoomFactor [2.0773052223433903]
- Offset [-119.91137569995328, 98.052613159122757]
- SceneRectInView [-119.91137569995328, 98.052613159122757, 1045.1040013999066, 571.89477368175449]
+ Offset [-119.91137569995328, 23.052613159122757]
+ SceneRectInView [-119.91137569995328, 23.052613159122757, 1045.1040013999066, 571.89477368175449]
}
Object {
$ObjectID 7
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
+ LoadSaveID "5014"
+ Extents [2171.0, 1188.0]
+ ZoomFactor [2.03]
+ Offset [-310.81869612068976, -180.61083743842369]
+ SceneRectInView [-310.81869612068976, -180.61083743842369, 1069.4581280788179, 585.22167487684737]
+ }
+ Object {
+ $ObjectID 8
+ IsActive [0]
+ IsTabbed [0]
+ ViewObjType "SimulinkSubsys"
+ LoadSaveID "4938"
+ Extents [2171.0, 1188.0]
+ ZoomFactor [2.9013369436452869]
+ Offset [-326.36442414255714, -208.23320111992533]
+ SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065]
+ }
+ Object {
+ $ObjectID 9
+ IsActive [0]
+ IsTabbed [0]
+ ViewObjType "SimulinkSubsys"
LoadSaveID "4850"
Extents [2171.0, 1188.0]
ZoomFactor [1.3964802319507956]
@@ -437,7 +460,7 @@ Model {
SceneRectInView [400.19641548607717, -203.35510808500248, 1554.6227940278457, 850.710216170005]
}
Object {
- $ObjectID 8
+ $ObjectID 10
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -448,7 +471,7 @@ Model {
SceneRectInView [12.87372526868586, 135.34665821934539, 468.38536196262828, 256.30668356130923]
}
Object {
- $ObjectID 9
+ $ObjectID 11
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -459,7 +482,7 @@ Model {
SceneRectInView [-158.02631578947376, -70.1842105263158, 571.31578947368428, 312.63157894736844]
}
Object {
- $ObjectID 10
+ $ObjectID 12
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -470,7 +493,7 @@ Model {
SceneRectInView [-158.02631578947376, -70.1842105263158, 571.31578947368428, 312.63157894736844]
}
Object {
- $ObjectID 11
+ $ObjectID 13
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -481,7 +504,7 @@ Model {
SceneRectInView [80.154339026122614, 154.55463376836479, 658.838253999605, 360.52503258937384]
}
Object {
- $ObjectID 12
+ $ObjectID 14
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -492,7 +515,7 @@ Model {
SceneRectInView [569.47947453590621, 441.73077216658527, 412.15823842818759, 225.53845566682949]
}
Object {
- $ObjectID 13
+ $ObjectID 15
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -503,7 +526,7 @@ Model {
SceneRectInView [-33.73965992647058, -45.831932773109244, 364.8739495798319, 199.66386554621849]
}
Object {
- $ObjectID 14
+ $ObjectID 16
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -514,7 +537,7 @@ Model {
SceneRectInView [82.52421875, 140.1, 217.1, 118.8]
}
Object {
- $ObjectID 15
+ $ObjectID 17
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -525,7 +548,7 @@ Model {
SceneRectInView [-608.35758121641425, -114.33481638525888, 1528.7515828078519, 836.55314618872774]
}
Object {
- $ObjectID 16
+ $ObjectID 18
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -536,7 +559,7 @@ Model {
SceneRectInView [-827.23464439655174, -71.967241379310337, 1390.5629310344827, 760.93448275862067]
}
Object {
- $ObjectID 17
+ $ObjectID 19
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -547,7 +570,7 @@ Model {
SceneRectInView [256.79612068965514, -277.26034482758621, 660.65775862068972, 361.52068965517242]
}
Object {
- $ObjectID 18
+ $ObjectID 20
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -558,7 +581,7 @@ Model {
SceneRectInView [-793.97389720558533, -290.08334454895737, 1059.8648857917149, 579.97212543554]
}
Object {
- $ObjectID 19
+ $ObjectID 21
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -569,7 +592,7 @@ Model {
SceneRectInView [92.0078125, -350.98802908437432, 2171.0, 1188.0]
}
Object {
- $ObjectID 20
+ $ObjectID 22
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -580,7 +603,7 @@ Model {
SceneRectInView [-112.83168536009441, -373.76948051948051, 901.91337072018882, 493.538961038961]
}
Object {
- $ObjectID 21
+ $ObjectID 23
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -591,7 +614,7 @@ Model {
SceneRectInView [-48.06941549217828, -144.92583198051949, 845.83414348435656, 462.851663961039]
}
Object {
- $ObjectID 22
+ $ObjectID 24
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -602,7 +625,7 @@ Model {
SceneRectInView [-706.68143309105676, -478.27262581168833, 1380.7144286821135, 755.54525162337666]
}
Object {
- $ObjectID 23
+ $ObjectID 25
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -613,7 +636,7 @@ Model {
SceneRectInView [96.146189545676577, -4.1074743527112787, 703.95762090864685, 385.21494870542256]
}
Object {
- $ObjectID 24
+ $ObjectID 26
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -624,7 +647,7 @@ Model {
SceneRectInView [-211.69612560424537, 252.15114629325387, 605.0, 331.06402579456471]
}
Object {
- $ObjectID 25
+ $ObjectID 27
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -635,7 +658,7 @@ Model {
SceneRectInView [-410.51953125, -620.5, 2171.0, 1188.0]
}
Object {
- $ObjectID 26
+ $ObjectID 28
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -646,7 +669,7 @@ Model {
SceneRectInView [-128.63423482247225, -154.52586206896552, 701.83189655172418, 384.05172413793105]
}
Object {
- $ObjectID 27
+ $ObjectID 29
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -657,7 +680,7 @@ Model {
SceneRectInView [-194.803483319316, -141.48435660218672, 1189.606966638632, 650.96871320437344]
}
Object {
- $ObjectID 28
+ $ObjectID 30
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -668,7 +691,7 @@ Model {
SceneRectInView [-1178.7732548019947, -284.77258542366184, 1369.7496346039895, 749.54517084732367]
}
Object {
- $ObjectID 29
+ $ObjectID 31
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -679,7 +702,7 @@ Model {
SceneRectInView [-116.92703117366869, -144.85219724177151, 889.14932180693916, 486.55430414861524]
}
Object {
- $ObjectID 30
+ $ObjectID 32
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -690,7 +713,7 @@ Model {
SceneRectInView [-736.65534376844744, 2332.9648668093268, 1240.5714285714287, 678.85714285714289]
}
Object {
- $ObjectID 31
+ $ObjectID 33
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -701,7 +724,7 @@ Model {
SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849]
}
Object {
- $ObjectID 32
+ $ObjectID 34
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -712,7 +735,7 @@ Model {
SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849]
}
Object {
- $ObjectID 33
+ $ObjectID 35
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -723,7 +746,7 @@ Model {
SceneRectInView [42.960073495100346, -92.586881707886135, 510.17360300979931, 279.17376341577227]
}
Object {
- $ObjectID 34
+ $ObjectID 36
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -734,7 +757,7 @@ Model {
SceneRectInView [-39.932291666666657, -42.75, 452.29166666666669, 247.5]
}
Object {
- $ObjectID 35
+ $ObjectID 37
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -745,7 +768,7 @@ Model {
SceneRectInView [-256.01528596010269, -463.59387395006996, 1774.7883844202054, 971.18774790013993]
}
Object {
- $ObjectID 36
+ $ObjectID 38
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -756,7 +779,7 @@ Model {
SceneRectInView [1438.5531788793103, 818.97931034482758, 778.56551724137933, 426.04137931034484]
}
Object {
- $ObjectID 37
+ $ObjectID 39
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -767,7 +790,7 @@ Model {
SceneRectInView [917.234213362069, -427.28793103448277, 975.078448275862, 533.57586206896553]
}
Object {
- $ObjectID 38
+ $ObjectID 40
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -778,7 +801,7 @@ Model {
SceneRectInView [-544.74878771551721, -352.2155172413793, 963.84913793103453, 527.43103448275861]
}
Object {
- $ObjectID 39
+ $ObjectID 41
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -789,7 +812,7 @@ Model {
SceneRectInView [374.06441393490434, 302.07339302379842, 1297.2149221301913, 709.85321395240317]
}
Object {
- $ObjectID 40
+ $ObjectID 42
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -800,7 +823,7 @@ Model {
SceneRectInView [732.82082435344819, -198.046551724138, 2023.1474137931036, 1107.093103448276]
}
Object {
- $ObjectID 41
+ $ObjectID 43
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -811,7 +834,7 @@ Model {
SceneRectInView [89.479636723777276, -28.60714285714289, 1240.5714285714287, 678.85714285714289]
}
Object {
- $ObjectID 42
+ $ObjectID 44
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -822,7 +845,7 @@ Model {
SceneRectInView [-132.82012310290415, -16.387940803946407, 723.66666666666663, 396.0]
}
Object {
- $ObjectID 43
+ $ObjectID 45
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -833,7 +856,7 @@ Model {
SceneRectInView [-165.54878048780483, -115.96864111498257, 756.44599303135885, 413.93728222996515]
}
Object {
- $ObjectID 44
+ $ObjectID 46
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -844,7 +867,7 @@ Model {
SceneRectInView [-221.49999999999994, -146.6, 868.4, 475.2]
}
Object {
- $ObjectID 45
+ $ObjectID 47
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -855,7 +878,7 @@ Model {
SceneRectInView [-173.20175438596488, -108.42105263157893, 761.75438596491222, 416.84210526315786]
}
Object {
- $ObjectID 46
+ $ObjectID 48
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -866,7 +889,7 @@ Model {
SceneRectInView [-173.20175438596488, -108.42105263157893, 761.75438596491222, 416.84210526315786]
}
Object {
- $ObjectID 47
+ $ObjectID 49
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -877,7 +900,7 @@ Model {
SceneRectInView [-164.07534246575341, -91.924657534246592, 743.49315068493149, 406.84931506849318]
}
Object {
- $ObjectID 48
+ $ObjectID 50
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -888,7 +911,7 @@ Model {
SceneRectInView [-173.20175438596488, -115.42105263157893, 761.75438596491222, 416.84210526315786]
}
Object {
- $ObjectID 49
+ $ObjectID 51
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -899,7 +922,7 @@ Model {
SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786]
}
Object {
- $ObjectID 50
+ $ObjectID 52
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -910,7 +933,7 @@ Model {
SceneRectInView [-165.70175438596488, -113.92105263157893, 761.75438596491222, 416.84210526315786]
}
Object {
- $ObjectID 51
+ $ObjectID 53
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -921,7 +944,7 @@ Model {
SceneRectInView [-327.9765625, -403.5, 2171.0, 1188.0]
}
Object {
- $ObjectID 52
+ $ObjectID 54
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -932,7 +955,7 @@ Model {
SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849]
}
Object {
- $ObjectID 53
+ $ObjectID 55
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -943,7 +966,7 @@ Model {
SceneRectInView [-33.739659926470551, -42.831932773109244, 364.8739495798319, 199.66386554621849]
}
Object {
- $ObjectID 54
+ $ObjectID 56
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -954,18 +977,7 @@ Model {
SceneRectInView [-43.454503676470551, -32.331932773109244, 364.8739495798319, 199.66386554621849]
}
Object {
- $ObjectID 55
- IsActive [0]
- IsTabbed [0]
- ViewObjType "SimulinkSubsys"
- LoadSaveID "4938"
- Extents [2171.0, 1188.0]
- ZoomFactor [2.9013369436452869]
- Offset [-326.36442414255714, -208.23320111992533]
- SceneRectInView [-326.36442414255714, -208.23320111992533, 748.27572328511428, 409.46640223985065]
- }
- Object {
- $ObjectID 56
+ $ObjectID 57
IsActive [0]
IsTabbed [0]
ViewObjType "SimulinkSubsys"
@@ -975,17 +987,6 @@ Model {
Offset [-116.23828124999997, -57.0]
SceneRectInView [-116.23828124999997, -57.0, 361.83333333333331, 198.0]
}
- Object {
- $ObjectID 57
- IsActive [0]
- IsTabbed [0]
- ViewObjType "SimulinkSubsys"
- LoadSaveID "5014"
- Extents [2171.0, 1188.0]
- ZoomFactor [2.03]
- Offset [-310.81869612068976, -180.61083743842369]
- SceneRectInView [-310.81869612068976, -180.61083743842369, 1069.4581280788179, 585.22167487684737]
- }
Object {
$ObjectID 58
IsActive [0]
@@ -1388,9 +1389,9 @@ Model {
ModifiedByFormat "%"
LastModifiedBy "nunoguedelha"
ModifiedDateFormat "%"
- LastModifiedDate "Fri May 07 17:27:52 2021"
- RTWModifiedTimeStamp 542309261
- ModelVersionFormat "%"
+ LastModifiedDate "Tue May 11 08:23:07 2021"
+ RTWModifiedTimeStamp 542621747
+ ModelVersionFormat "%"
SampleTimeColors on
SampleTimeAnnotations on
LibraryLinkDisplay "disabled"
@@ -2676,7 +2677,7 @@ Model {
TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
TiledPageScale 1
ShowPageBoundaries off
- ZoomFactor "206"
+ ZoomFactor "237"
ReportName "simulink-default.rpt"
SIDHighWatermark "5020"
SimulinkSubDomain "Simulink"
@@ -2685,7 +2686,7 @@ Model {
Name "Bus\nCreator"
SID "4944"
Ports [6, 1]
- Position [1730, 400, 1735, 570]
+ Position [1565, 400, 1570, 570]
ZOrder 1276
Inputs "6"
DisplayOption "bar"
@@ -2696,7 +2697,7 @@ Model {
Name "Bus\nSelector1"
SID "5013"
Ports [1, 2]
- Position [1465, 396, 1470, 454]
+ Position [1450, 396, 1455, 454]
ZOrder 1290
OutputSignals "joints_position,joints_velocity"
Port {
@@ -2715,7 +2716,7 @@ Model {
Ports [1, 6]
Position [760, 404, 765, 596]
ZOrder 1277
- OutputSignals "joints_position,joints_velocity,nuDot,signal6,wrench_RFoot,wrench_LFoot"
+ OutputSignals "joints_position,joints_velocity,nuDot,imuOut,wrench_RFoot,wrench_LFoot"
Port {
PortNumber 1
Name ""
@@ -2730,7 +2731,7 @@ Model {
}
Port {
PortNumber 4
- Name ""
+ Name ""
}
Port {
PortNumber 5
@@ -2770,6 +2771,12 @@ Model {
ContentPreviewEnabled off
ConfigSource "Workspace"
ConfigObject "'WBTConfigRobot'"
+ RobotName "'icubSim'"
+ UrdfFile "'model.urdf'"
+ ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}"
+ ControlBoardsNames "{'left_arm','right_arm','torso'}"
+ LocalName "'WBT'"
+ GravityVector "[0,0,-9.81]"
}
Block {
BlockType Constant
@@ -2784,7 +2791,8 @@ Model {
$ClassName "Simulink.Mask"
Type "Fixed neck position"
Description "we assume that the neck position is fixed (\"lumped\" head and torso) in the [0 0 0] position."
- Display "image('images/initNeckPosBlock.jpeg'); % Use \"Edit Image\" on toolstrip to change."
+ Display "image(strcat(fileparts(which(bdroot)),'/images/initNeckPosBlock.jpeg')); % Use \"Edit Image\" on toolstrip"
+ " to change.\n"
RunInitForIconRedraw "off"
Object {
$PropName "DialogControls"
@@ -19005,165 +19013,7 @@ Model {
}
Block {
BlockType SubSystem
- Name "Robot Visualizer"
- SID "5014"
- Ports [1]
- Position [1465, 245, 1725, 365]
- ZOrder 1291
- TreatAsAtomicUnit on
- SystemSampleTime "confVisualizer.tStep"
- RequestExecContextInheritance off
- ContentPreviewEnabled on
- System {
- Name "Robot Visualizer"
- Location [-75, -1417, 2485, 0]
- SystemRect [0.000000, 0.000000, 0.000000, 0.000000]
- Open off
- PortBlocksUseCompactNotation off
- SetExecutionDomain off
- ExecutionDomainType "Deduce"
- ModelBrowserVisibility on
- ModelBrowserWidth 200
- ScreenColor "white"
- PaperOrientation "landscape"
- PaperPositionMode "auto"
- PaperType "usletter"
- PaperUnits "inches"
- TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
- TiledPageScale 1
- ShowPageBoundaries off
- ZoomFactor "203"
- SimulinkSubDomain "Simulink"
- Block {
- BlockType Inport
- Name "In1"
- SID "5015"
- Position [-35, 73, -5, 87]
- ZOrder 1276
- }
- Block {
- BlockType BusSelector
- Name "Bus\nSelector"
- SID "4943"
- Ports [1, 4]
- Position [45, 24, 50, 136]
- ZOrder 1275
- OutputSignals "w_H_b,basePose_dot,joints_position,joints_velocity"
- Port {
- PortNumber 1
- Name ""
- }
- Port {
- PortNumber 2
- Name ""
- }
- Port {
- PortNumber 3
- Name ""
- }
- Port {
- PortNumber 4
- Name ""
- }
- }
- Block {
- BlockType Reference
- Name "RobotVisualizer"
- SID "4939"
- Ports [4]
- Position [210, 22, 430, 138]
- ZOrder 1271
- LibraryVersion "1.4"
- SourceBlock "mwbs_visualizers_lib/RobotVisualizer"
- SourceType "mwbs.Visualizers.robotVisualizer.RobotVisualizer"
- SourceProductName "Matlab Whole-body Simulator"
- config "confVisualizer"
- SimulateUsing "Interpreted execution"
- }
- Line {
- Name ""
- ZOrder 308
- Labels [0, 0]
- SrcBlock "Bus\nSelector"
- SrcPort 2
- DstBlock "RobotVisualizer"
- DstPort 2
- }
- Line {
- ZOrder 446
- SrcBlock "In1"
- SrcPort 1
- DstBlock "Bus\nSelector"
- DstPort 1
- }
- Line {
- Name ""
- ZOrder 307
- Labels [0, 0]
- SrcBlock "Bus\nSelector"
- SrcPort 3
- DstBlock "RobotVisualizer"
- DstPort 3
- }
- Line {
- Name ""
- ZOrder 306
- Labels [0, 0]
- SrcBlock "Bus\nSelector"
- SrcPort 1
- DstBlock "RobotVisualizer"
- DstPort 1
- }
- Line {
- Name ""
- ZOrder 309
- Labels [0, 0]
- SrcBlock "Bus\nSelector"
- SrcPort 4
- DstBlock "RobotVisualizer"
- DstPort 4
- }
- }
- }
- Block {
- BlockType Selector
- Name "Selector"
- SID "4963"
- Ports [1, 1]
- Position [840, 473, 880, 497]
- ZOrder 1280
- InputPortWidth "6+robot_config.N_DOF"
- IndexOptions "Index vector (dialog)"
- Indices "[7:6+Config.N_DOF]"
- OutputSizes "1"
- }
- Block {
- BlockType Selector
- Name "Selector1"
- SID "4975"
- Ports [1, 1]
- Position [840, 443, 880, 467]
- ZOrder 1281
- InputPortWidth "robot_config.N_DOF"
- IndexOptions "Index vector (dialog)"
- Indices "[1:Config.N_DOF]"
- OutputSizes "1"
- }
- Block {
- BlockType Selector
- Name "Selector2"
- SID "4976"
- Ports [1, 1]
- Position [840, 413, 880, 437]
- ZOrder 1282
- InputPortWidth "robot_config.N_DOF"
- IndexOptions "Index vector (dialog)"
- Indices "[1:Config.N_DOF]"
- OutputSizes "1"
- }
- Block {
- BlockType SubSystem
- Name "Subsystem"
+ Name "Robot Simulator"
SID "4955"
Ports [1, 5]
Position [1190, 427, 1385, 573]
@@ -19175,7 +19025,7 @@ Model {
RequestExecContextInheritance off
ContentPreviewEnabled on
System {
- Name "Subsystem"
+ Name "Robot Simulator"
Location [-75, -1417, 2485, 0]
SystemRect [0.000000, 0.000000, 0.000000, 0.000000]
Open off
@@ -19260,6 +19110,12 @@ Model {
ContentPreviewEnabled on
ConfigSource "Workspace"
ConfigObject "'WBTConfigRobotSim'"
+ RobotName "'icubSim'"
+ UrdfFile "'model.urdf'"
+ ControlledJoints "{'l_elbow','l_shoulder_pitch','torso_roll'}"
+ ControlBoardsNames "{'left_arm','right_arm','torso'}"
+ LocalName "'WBT'"
+ GravityVector "[0,0,-9.81]"
}
Block {
BlockType Constant
@@ -19278,28 +19134,9 @@ Model {
Value "motorsReflectedInertia"
VectorParams1D off
}
- Block {
- BlockType Reference
- Name "IMUsensor"
- SID "4938"
- Ports [4, 1]
- Position [545, 392, 765, 508]
- ZOrder 1270
- LibraryVersion "1.17"
- SourceBlock "mwbs_robotSensors_lib/IMUsensor"
- SourceType "IMUsensor"
- SourceProductName "Matlab Whole-body Simulator"
- RTWMemSecFuncInitTerm "Inherit from model"
- RTWMemSecFuncExecute "Inherit from model"
- RTWMemSecDataConstants "Inherit from model"
- RTWMemSecDataInternal "Inherit from model"
- RTWMemSecDataParameters "Inherit from model"
- ContentPreviewEnabled on
- frameName "robot_config.robotFrames.IMU"
- }
Block {
BlockType SubSystem
- Name "MATLAB Function"
+ Name "Friction Model"
SID "5016"
Ports [2, 1]
Position [-5, 197, 110, 328]
@@ -19311,7 +19148,7 @@ Model {
SFBlockType "MATLAB Function"
ContentPreviewEnabled on
System {
- Name "MATLAB Function"
+ Name "Friction Model"
Location [223, 338, 826, 833]
SystemRect [0.000000, 0.000000, 0.000000, 0.000000]
Open off
@@ -19427,6 +19264,30 @@ Model {
}
}
}
+ Block {
+ BlockType Reference
+ Name "IMUsensor"
+ SID "4938"
+ Ports [4, 1]
+ Position [545, 392, 765, 508]
+ ZOrder 1270
+ HideAutomaticName off
+ LibraryVersion "1.17"
+ SourceBlock "mwbs_robotSensors_lib/IMUsensor"
+ SourceType "IMUsensor"
+ SourceProductName "Matlab Whole-body Simulator"
+ RTWMemSecFuncInitTerm "Inherit from model"
+ RTWMemSecFuncExecute "Inherit from model"
+ RTWMemSecDataConstants "Inherit from model"
+ RTWMemSecDataInternal "Inherit from model"
+ RTWMemSecDataParameters "Inherit from model"
+ ContentPreviewEnabled on
+ frameName "robot_config.robotFrames.IMU"
+ Port {
+ PortNumber 1
+ Name "imuOut"
+ }
+ }
Block {
BlockType Memory
Name "Memory"
@@ -19446,6 +19307,7 @@ Model {
ZOrder 1231
ForegroundColor "red"
BackgroundColor "yellow"
+ HideAutomaticName off
LibraryVersion "3.20"
SourceBlock "mwbs_robotDynamicsWithContacts_lib/RobotDynWithContacts"
SourceType ""
@@ -19486,7 +19348,7 @@ Model {
}
Block {
BlockType Outport
- Name ""
+ Name "nuDot"
SID "4960"
Position [870, 338, 900, 352]
ZOrder 1279
@@ -19525,7 +19387,9 @@ Model {
}
}
Line {
+ Name "imuOut"
ZOrder 326
+ Labels [0, 0]
SrcBlock "IMUsensor"
SrcPort 1
Points [48, 0; 0, -75]
@@ -19591,7 +19455,7 @@ Model {
Branch {
ZOrder 325
Points [0, -150]
- DstBlock ""
+ DstBlock "nuDot"
DstPort 1
}
Branch {
@@ -19611,12 +19475,12 @@ Model {
ZOrder 347
SrcBlock "joints torque"
SrcPort 1
- DstBlock "MATLAB Function"
+ DstBlock "Friction Model"
DstPort 2
}
Line {
ZOrder 348
- SrcBlock "MATLAB Function"
+ SrcBlock "Friction Model"
SrcPort 1
DstBlock "RobotDynWithContacts"
DstPort 1
@@ -19635,16 +19499,174 @@ Model {
SrcBlock "Memory"
SrcPort 1
Points [-164, 0; 0, 55]
- DstBlock "MATLAB Function"
+ DstBlock "Friction Model"
DstPort 1
}
}
}
+ Block {
+ BlockType SubSystem
+ Name "Robot Visualizer"
+ SID "5014"
+ Ports [1]
+ Position [1310, 225, 1570, 345]
+ ZOrder 1291
+ TreatAsAtomicUnit on
+ SystemSampleTime "confVisualizer.tStep"
+ RequestExecContextInheritance off
+ ContentPreviewEnabled on
+ System {
+ Name "Robot Visualizer"
+ Location [-75, -1417, 2485, 0]
+ SystemRect [0.000000, 0.000000, 0.000000, 0.000000]
+ Open off
+ PortBlocksUseCompactNotation off
+ SetExecutionDomain off
+ ExecutionDomainType "Deduce"
+ ModelBrowserVisibility on
+ ModelBrowserWidth 200
+ ScreenColor "white"
+ PaperOrientation "landscape"
+ PaperPositionMode "auto"
+ PaperType "usletter"
+ PaperUnits "inches"
+ TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000]
+ TiledPageScale 1
+ ShowPageBoundaries off
+ ZoomFactor "203"
+ SimulinkSubDomain "Simulink"
+ Block {
+ BlockType Inport
+ Name "In1"
+ SID "5015"
+ Position [-35, 73, -5, 87]
+ ZOrder 1276
+ }
+ Block {
+ BlockType BusSelector
+ Name "Bus\nSelector"
+ SID "4943"
+ Ports [1, 4]
+ Position [45, 24, 50, 136]
+ ZOrder 1275
+ OutputSignals "w_H_b,basePose_dot,joints_position,joints_velocity"
+ Port {
+ PortNumber 1
+ Name ""
+ }
+ Port {
+ PortNumber 2
+ Name ""
+ }
+ Port {
+ PortNumber 3
+ Name ""
+ }
+ Port {
+ PortNumber 4
+ Name ""
+ }
+ }
+ Block {
+ BlockType Reference
+ Name "RobotVisualizer"
+ SID "4939"
+ Ports [4]
+ Position [210, 22, 430, 138]
+ ZOrder 1271
+ LibraryVersion "1.4"
+ SourceBlock "mwbs_visualizers_lib/RobotVisualizer"
+ SourceType "mwbs.Visualizers.robotVisualizer.RobotVisualizer"
+ SourceProductName "Matlab Whole-body Simulator"
+ config "confVisualizer"
+ SimulateUsing "Interpreted execution"
+ }
+ Line {
+ Name ""
+ ZOrder 308
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector"
+ SrcPort 2
+ DstBlock "RobotVisualizer"
+ DstPort 2
+ }
+ Line {
+ ZOrder 446
+ SrcBlock "In1"
+ SrcPort 1
+ DstBlock "Bus\nSelector"
+ DstPort 1
+ }
+ Line {
+ Name ""
+ ZOrder 307
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector"
+ SrcPort 3
+ DstBlock "RobotVisualizer"
+ DstPort 3
+ }
+ Line {
+ Name ""
+ ZOrder 306
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector"
+ SrcPort 1
+ DstBlock "RobotVisualizer"
+ DstPort 1
+ }
+ Line {
+ Name ""
+ ZOrder 309
+ Labels [0, 0]
+ SrcBlock "Bus\nSelector"
+ SrcPort 4
+ DstBlock "RobotVisualizer"
+ DstPort 4
+ }
+ }
+ }
+ Block {
+ BlockType Selector
+ Name "Selector"
+ SID "4963"
+ Ports [1, 1]
+ Position [840, 473, 880, 497]
+ ZOrder 1280
+ InputPortWidth "6+robot_config.N_DOF"
+ IndexOptions "Index vector (dialog)"
+ Indices "[7:6+Config.N_DOF]"
+ OutputSizes "1"
+ }
+ Block {
+ BlockType Selector
+ Name "Selector1"
+ SID "4975"
+ Ports [1, 1]
+ Position [840, 443, 880, 467]
+ ZOrder 1281
+ InputPortWidth "robot_config.N_DOF"
+ IndexOptions "Index vector (dialog)"
+ Indices "[1:Config.N_DOF]"
+ OutputSizes "1"
+ }
+ Block {
+ BlockType Selector
+ Name "Selector2"
+ SID "4976"
+ Ports [1, 1]
+ Position [840, 413, 880, 437]
+ ZOrder 1282
+ InputPortWidth "robot_config.N_DOF"
+ IndexOptions "Index vector (dialog)"
+ Indices "[1:Config.N_DOF]"
+ OutputSizes "1"
+ }
Line {
ZOrder 321
SrcBlock "Bus\nCreator"
SrcPort 1
- Points [19, 0; 0, 161; -1039, 0; 0, -146]
+ Points [48, 0; 0, 161; -903, 0; 0, -146]
DstBlock "Bus\nSelector2"
DstPort 1
}
@@ -19657,12 +19679,12 @@ Model {
}
Line {
ZOrder 362
- SrcBlock "Subsystem"
+ SrcBlock "Robot Simulator"
SrcPort 1
Points [19, 0; 0, -15]
Branch {
ZOrder 449
- Points [0, -120]
+ Points [0, -53; -123, 0; 0, -87]
DstBlock "Robot Visualizer"
DstPort 1
}
@@ -19674,34 +19696,34 @@ Model {
}
Line {
ZOrder 363
- SrcBlock "Subsystem"
+ SrcBlock "Robot Simulator"
SrcPort 2
DstBlock "Bus\nCreator"
DstPort 3
}
Line {
ZOrder 364
- SrcBlock "Subsystem"
+ SrcBlock "Robot Simulator"
SrcPort 3
DstBlock "Bus\nCreator"
DstPort 4
}
Line {
ZOrder 365
- SrcBlock "Subsystem"
+ SrcBlock "Robot Simulator"
SrcPort 4
DstBlock "Bus\nCreator"
DstPort 5
}
Line {
ZOrder 366
- SrcBlock "Subsystem"
+ SrcBlock "Robot Simulator"
SrcPort 5
DstBlock "Bus\nCreator"
DstPort 6
}
Line {
- Name ""
+ Name ""
ZOrder 379
Labels [0, 0]
SrcBlock "Bus\nSelector2"
@@ -19786,7 +19808,7 @@ Model {
ZOrder 437
SrcBlock "Memory"
SrcPort 1
- DstBlock "Subsystem"
+ DstBlock "Robot Simulator"
DstPort 1
}
Line {
@@ -20492,7 +20514,7 @@ Stateflow {
chart {
id 36
machine 1
- name "Subsystem/MATLAB Function"
+ name "Robot Simulator/Friction Model"
windowPosition [422 539.941 189 413]
viewLimits [0 156.75 0 153.75]
screen [1 1 3600 1200 1.180555555555556]
@@ -20660,7 +20682,7 @@ Stateflow {
instance {
id 44
machine 1
- name "Subsystem/MATLAB Function"
+ name "Robot Simulator/Friction Model"
chart 36
}
chart {
diff --git a/controllers/README.md b/controllers/README.md
index a45a320c..ddf92b6b 100644
--- a/controllers/README.md
+++ b/controllers/README.md
@@ -7,8 +7,8 @@ Simulink controllers for humanoid robots.
- **fixed-base-joints-torque-control** [[README]](fixed-base-joints-torque-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)
+- **floating-base-balancing-torque-control-with-simulator** [[README]](floating-base-balancing-torque-control-with-simulator/README.md)
- **floating-base-jerk-control** [[README]](floating-base-jerk-control/README.md)
-- **simulink-balancing-simulator** [[README]](simulink-balancing-simulator/README.md)
## Plotting Utilities
diff --git a/controllers/floating-base-balancing-torque-control-with-simulator/README.md b/controllers/floating-base-balancing-torque-control-with-simulator/README.md
deleted file mode 100644
index b5214cf1..00000000
--- a/controllers/floating-base-balancing-torque-control-with-simulator/README.md
+++ /dev/null
@@ -1,29 +0,0 @@
-## Module description
-
-This module implements a torque control balancing strategy. It computes the interaction forces at the feet in order to stabilise a desired `centroidal momentum` dynamics, which implies the tracking of a desired center-of-mass trajectory. A cost function penalizing high joint torques - that generate the feet forces - is added to the control framework.
-
-
-
-For details see also: [iCub whole-body control through force regulation on rigid non-coplanar contacts](http://journal.frontiersin.org/article/10.3389/frobt.2015.00006/abstract) and [Stability Analysis and Design of Momentum-Based Controllers for Humanoid Robots](https://ieeexplore.ieee.org/document/7759126).
-
-### Compatibility
-
-The folder contains the Simulink model `torqueControlBalancing.mdl`, which is generated by using Matlab R2017b.
-
-### Supported robots
-
-Currently, supported robots are: `iCubGenova04`, `iCubGenova02`, `icubGazeboSim`, `iCubGazeboV2_5`.
-
-## Module details
-
-### How to run the demo
-
-For information on how to use the controllers both in **simulation** and with the **real robot**, please refer to the **wiki** of the repo.
-
-### Configuration file
-
-At start, the module calls the initialization file initTorqueControlBalancing.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`.
diff --git a/doc/pics/robotSimulator_w_SampleRates.png b/doc/pics/robotSimulator_w_SampleRates.png
new file mode 100644
index 00000000..116845c1
Binary files /dev/null and b/doc/pics/robotSimulator_w_SampleRates.png differ
diff --git a/doc/pics/torqueControlwithSimu.png b/doc/pics/torqueControlwithSimu.png
new file mode 100644
index 00000000..9464c49d
Binary files /dev/null and b/doc/pics/torqueControlwithSimu.png differ
diff --git a/doc/pics/torqueControlwithSimu_w_SampleRates.png b/doc/pics/torqueControlwithSimu_w_SampleRates.png
new file mode 100644
index 00000000..d9aa6897
Binary files /dev/null and b/doc/pics/torqueControlwithSimu_w_SampleRates.png differ