diff --git a/CMakeLists.txt b/CMakeLists.txt index acd0156d3..7b99e7077 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,17 +1,20 @@ -# Copyright (C) 2013-2015 Instituto Italiano di Tecnologia -# Author: Jorhabib Eljaik, Francesco Romano +# Copyright (C) 2013-2017 iCub Facility - Istituto Italiano di Tecnologia +# Author: Jorhabib Eljaik, Francesco Romano, Diego Ferigo # CopyPolicy: Released under the terms of the GNU GPL v2.0. -cmake_minimum_required(VERSION 2.8.11) - +cmake_minimum_required(VERSION 3.3) project(WB-Toolbox) +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + find_package(YCM REQUIRED) include(YCMDefaultDirs) set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) set(WB-TOOLBOX_SHARE_DIR "${CMAKE_INSTALL_PREFIX}/share/WB-Toolbox") +add_subdirectory(MxAnyType) add_subdirectory(toolbox) install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/images DESTINATION ${WB-TOOLBOX_SHARE_DIR}) diff --git a/MxAnyType/CMakeLists.txt b/MxAnyType/CMakeLists.txt new file mode 100644 index 000000000..608670096 --- /dev/null +++ b/MxAnyType/CMakeLists.txt @@ -0,0 +1,51 @@ +cmake_policy(SET CMP0048 NEW) +project(MxAnyType LANGUAGES CXX VERSION 0.1) +cmake_minimum_required(VERSION 3.0.2) + +# Configure the project +# ===================== +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +include(GNUInstallDirs) +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# Use a general prefix for the project +set(VARS_PREFIX ${PROJECT_NAME}) +# set(VARS_PREFIX "MxAnyType") + +# Build the library +# ================= + +# Export the includes needed to who inherits this library +# Set this up properly for handling either BUILD and INSTALL trees +set(${VARS_PREFIX}_INCLUDE_BUILD ${CMAKE_CURRENT_SOURCE_DIR}/include) +set(${VARS_PREFIX}_INCLUDE_INSTALL ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX}) + +# Add the target +add_library(${VARS_PREFIX} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/include/AnyType.h + ${CMAKE_CURRENT_SOURCE_DIR}/include/MxAnyType.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/MxAnyType.cpp) + + +# TODO: temporary, waiting the library becomes a shared +if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") + set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") +endif() + +# Find Matlab resources +find_package(Matlab REQUIRED MX_LIBRARY) +target_include_directories(${VARS_PREFIX} SYSTEM PUBLIC "${Matlab_INCLUDE_DIRS}") + +# Setup the include directories +# TODO why in the how to was INTERFACE? +target_include_directories(${VARS_PREFIX} PUBLIC + $ + $) + +# Assign some useful properties +# set_target_properties(${VARS_PREFIX} PROPERTIES VERSION ${PROJECT_VERSION} +# PUBLIC_HEADER MxAnyType.h) + +# Build tests +# =========== +# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/tests) diff --git a/MxAnyType/include/AnyType.h b/MxAnyType/include/AnyType.h new file mode 100644 index 000000000..a13d1d03a --- /dev/null +++ b/MxAnyType/include/AnyType.h @@ -0,0 +1,61 @@ +#ifndef ANYTYPE_H +#define ANYTYPE_H + +#include +#include +#include + +class AnyType; + +typedef std::shared_ptr AnyTypeSPtr; +typedef std::vector AnyCell; +typedef std::unordered_map AnyStruct; + +class AnyType +{ +protected: + +public: + AnyType() = default; + virtual ~AnyType() = default; + + // Integers + virtual bool asInt(int& i) = 0; + // virtual bool asInt8(int8_t& i) = 0; + // virtual bool asInt16(int16_t& i) = 0; + virtual bool asInt32(int32_t& i) = 0; + // virtual bool asInt64(int64_t& i) = 0; + + // Unsigned Integers + virtual bool asUInt(unsigned& i) = 0; + // virtual bool asUInt8(uint8_t& i) = 0; + // virtual bool asUInt16(uint16_t& i) = 0; + // virtual bool asUInt32(uint32_t& i) = 0; + // virtual bool asUInt64(uint64_t& i) = 0; + + // Boolean + virtual bool asBool(bool& b) = 0; + + // Floating-point + // virtual bool asFloat(float& f) = 0; + virtual bool asDouble(double& d) = 0; + + // Characters + virtual bool asString(std::string& s) = 0; + + // Struct + virtual bool asAnyStruct(AnyStruct& map) = 0; + + // Cell array + virtual bool asAnyCell(AnyCell& map) = 0; + + // Matrix + // TODO: constraint max 2-dimension + // virtual bool asMatrixFloat(Eigen::MatrixXf mat) = 0; + // virtual bool asMatrixDouble(Eigen::MatrixXd mat) = 0; + + // Vector + virtual bool asVectorDouble(std::vector& vec) = 0; +}; + +#endif /* ANYTYPE_H */ diff --git a/MxAnyType/include/MxAnyType.h b/MxAnyType/include/MxAnyType.h new file mode 100644 index 000000000..b8990b741 --- /dev/null +++ b/MxAnyType/include/MxAnyType.h @@ -0,0 +1,98 @@ +#ifndef MXANYTYPE_H +#define MXANYTYPE_H + +#include +#include +#include +#include +#include "AnyType.h" +#include "matrix.h" + +class MxAnyType; + +// If needed in the future +// class MxAnyCell : public AnyCell {}; +// class MxAnyStruct : public AnyStruct {}; + +struct MxArrayMetadata { + mxClassID id; + bool isScalar; + size_t rows; + size_t cols; + size_t nElem; + size_t nDims; + std::vector dims; +}; + +class MxAnyType : public AnyType +{ +private: + const mxArray* mx; + MxArrayMetadata md; + bool validate; + + // TODO: https://it.mathworks.com/help/matlab/apiref/mxgetscalar.html returns a double always + bool asScalar(double& d); + bool validateClassId(mxClassID id1, mxClassID id2); + + // mxArray* getMxArrayPtr() const { return static_cast(getAnyDataPtr()); } + +public: + // void* getAnyDataPtr() const override { + // return (void*) mx; + // } + + MxAnyType(); + MxAnyType(const mxArray* m, bool validateId = false); + ~MxAnyType() = default; + MxAnyType(const MxAnyType& mxAnyType); + + void enableClassIDValidation(); + + // STRING / CHARS + // ============== + + bool asString(std::string& s) override; + + // SCALAR TYPES + // ============ + + // Generic casting + // --------------- + + bool asInt(int& i) override; + bool asUInt(unsigned& i) override; + + // Specific casting + // ---------------- + + bool asInt32(int32_t& i) override; + + // TODO: complete with all the other scalar types + // bool asInt64(int64_t& i) override + // { + // double buffer; + // if (!asScalar(buffer)) return false; + // i = static_cast(buffer); + // return validateClassId(md.id, mxINT64_CLASS); + // } + + bool asDouble(double& d) override; + bool asBool(bool& b) override; + + // COMPOSITE DATA TYPES + // ==================== + + bool asAnyStruct(AnyStruct& s) override; + bool asAnyCell(AnyCell& cell) override; + + // MATRIX + // ====== + + // VECTOR + // ====== + + bool asVectorDouble(std::vector& vec) override; +}; + +#endif /* MXANYTYPE_H */ diff --git a/MxAnyType/src/MxAnyType.cpp b/MxAnyType/src/MxAnyType.cpp new file mode 100644 index 000000000..55e7fe61c --- /dev/null +++ b/MxAnyType/src/MxAnyType.cpp @@ -0,0 +1,207 @@ +#include "MxAnyType.h" +#include + +// PRIVATE METHODS +// =============== + +bool MxAnyType::asScalar(double& d) +{ + if (!mx) return false; + if (!mxIsScalar(mx)) return false; // 1x1 + if (!mxIsNumeric(mx)) return false; // Types: https://it.mathworks.com/help/matlab/apiref/mxisnumeric.html + + // Cast to double since even a mxINT8_CLASS is returned as double: + // https://it.mathworks.com/help/matlab/apiref/mxgetscalar.html + d = static_cast(mxGetScalar(mx)); + return true; +} + +bool MxAnyType::validateClassId(mxClassID id1, mxClassID id2) +{ + if (validate) + return id1 == id2; + else + return true; +} + +// PUBLIC METHODS +// ============== + +// Constructors +// ============ + +MxAnyType::MxAnyType() : mx(nullptr), validate(false) +{ + md.id = mxUNKNOWN_CLASS; +} + +MxAnyType::MxAnyType(const mxArray* m, bool validateId) : mx(m), validate(validateId) +{ + assert(mx); + + // Get the ID + md.id = mxGetClassID(mx); + assert(md.id != mxVOID_CLASS); + assert(md.id != mxUNKNOWN_CLASS); + + // Get the other metadata + md.isScalar = mxIsScalar(mx); + md.rows = static_cast(mxGetN(mx)); + md.cols = static_cast(mxGetM(mx)); + md.nElem = static_cast(mxGetNumberOfElements(mx)); + md.nDims = static_cast(mxGetNumberOfDimensions(mx)); + + if (md.isScalar) + assert(md.rows == md.cols == md.nElem == 1); + + // TODO: only 2 dims currently supported + assert(md.nDims == 2); + assert(md.rows * md.cols == md.nElem); + + const size_t* size = mxGetDimensions(mx); + for (unsigned dim = 0; dim < md.nDims; ++dim) { + md.dims.push_back(static_cast(size[dim])); + } + assert(md.dims.size() == 2); +} + +MxAnyType::MxAnyType(const MxAnyType& mxAnyType) +: mx(mxAnyType.mx) +, md(mxAnyType.md) +, validate(mxAnyType.validate) +{} + +void MxAnyType::enableClassIDValidation() +{ + validate = true; +} + +// STRING / CHARS +// ============== + +bool MxAnyType::asString(std::string& s) +{ + if (!mx) return false; + if (md.id != mxCHAR_CLASS) return false; + char* buffer = mxArrayToString(mx); + s = std::string(buffer); + mxFree(buffer); + return true; +} + +// SCALAR TYPES +// ============ + +// Generic casting +// --------------- + +bool MxAnyType::asInt(int& i) +{ + double buffer; + if (!asScalar(buffer)) return false; + i = static_cast(buffer); + return true; +} + +bool MxAnyType::asUInt(unsigned& i) +{ + double buffer; + if (!asScalar(buffer)) return false; + i = static_cast(buffer); + return true; +} + +// Specific casting +// ---------------- + +bool MxAnyType::asInt32(int32_t& i) +{ + double buffer; + if (!asScalar(buffer)) return false; + i = static_cast(buffer); + return validateClassId(md.id, mxINT32_CLASS); +} + +// TODO: complete with all the other scalar types +// bool asInt64(int64_t& i) +// { +// double buffer; +// if (!asScalar(buffer)) return false; +// i = static_cast(buffer); +// return validateClassId(md.id, mxINT64_CLASS); +// } + +bool MxAnyType::asDouble(double& d) +{ + bool ok = asScalar(d); + return ok && validateClassId(md.id, mxDOUBLE_CLASS); +} + +bool MxAnyType::asBool(bool& b) + { + if (!mx) return false; + if (!mxIsLogicalScalar(mx)) return false; + b = mxIsLogicalScalarTrue(mx); + return true; +} + +// COMPOSITE DATA TYPES +// ==================== + +bool MxAnyType::asAnyStruct(AnyStruct& s) +{ + if (!mx) return false; + if (md.id != mxSTRUCT_CLASS) return false; + + for (unsigned i = 0; i < mxGetNumberOfFields(mx); ++i) { + const char* fieldName = mxGetFieldNameByNumber(mx,i); + // TODO multidimensional struct + mxArray* fieldContent = mxGetFieldByNumber(mx,0,i); + if (fieldName == nullptr) return false; + if (fieldContent == nullptr) return false; + s[std::string(fieldName)] = std::make_shared(fieldContent); + } + return true; +} + +bool MxAnyType::asAnyCell(AnyCell& cell) +{ + if (!mx) return false; + if (md.id != mxCELL_CLASS) return false; + + // TODO: AnyCell then will have a operator()(3,4) method; + for (unsigned i=0; i < mxGetNumberOfElements(mx); ++i) { + mxArray* cellContent = mxGetCell(mx, i); + if (!cellContent) return false; + cell.push_back(std::make_shared(cellContent)); + } + return true; +} + +// MATRIX +// ====== + +// VECTOR +// ====== + +// TODO: +// Tests with other types (uint8) https://it.mathworks.com/help/matlab/apiref/mxgetdata.html (Description) +bool MxAnyType::asVectorDouble(std::vector& vec) +{ + if (!mx) return false; + if (!mxIsDouble(mx)) return false; + + if (md.rows > 1 && md.cols > 1) { + return false; + } + + // TODO add method for complex vectors (and move the check into md) + if (mxIsComplex(mx)) return false; + + double* buffer = mxGetPr(mx); + if (!buffer) return false; + + vec.reserve(md.rows * md.cols); + vec.assign(buffer, buffer + md.rows * md.cols); + return true; +} diff --git a/README.md b/README.md index 34b2187de..4478dee41 100644 --- a/README.md +++ b/README.md @@ -1,67 +1,76 @@ ![](http://drive.google.com/uc?export=view&id=0B6zDGh11iY6oc0gtM0lMdDNweWM) -Whole Body Toolbox - A Simulink Toolbox for Whole Body Control -------------------------------------------------------------- +# Whole Body Toolbox +

A Simulink Toolbox for Whole Body Control

[![GitPitch](https://gitpitch.com/assets/badge.svg)](https://gitpitch.com/robotology/WB-Toolbox?grs=github) -### This toolbox is intended as a replacement (a 2.0 version) of the [WBI-Toolbox](https://github.com/robotology-playground/WBI-Toolbox). If you are interested, see [the migration guide from WBI-Toolbox 1.0](doc/Migration.md). - ### Main Goal -> The library should allow non-programming experts or those researchers just getting acquainted with Whole Body Control to more easily deploy controllers either on simulation or a real YARP-based robotic platform, as well as to analyze their performance and take advantage of the innumerable MATLAB and Simulink toolboxes. We like to call it "rapid controller prototyping" after which a proper YARP module should be made for hard real time performance and final deployment. + +> The library should allow non-programming experts or those researchers just getting acquainted with Whole Body Control to more easily deploy controllers either on simulation or real YARP-based robotic platforms, and to analyze their performance taking advantage of the innumerable MATLAB and Simulink toolboxes. We like to call it "rapid controller prototyping" after which a proper YARP module should be made for hard real time performance and final deployment. The following video shows CoDyCo's latest results on iCub in which the top level controller has been implemented with the [WB-Toolbox](https://github.com/robotology/WB-Toolbox) running at a 10ms rate!

iCub balancing on one foot via external force control and interacting with humans

- - [Installation](https://github.com/robotology/WB-Toolbox#Installation) - [Troubleshooting](https://github.com/robotology/WB-Toolbox#Troubleshooting) -- [Using the Toolbox](https://github.com/robotology/WB-Toolbox#Using the Toolbox) +- [Using the Toolbox](https://github.com/robotology/WB-Toolbox#using-the-toolbox) +- [Migrating from WBI-Toolbox](https://github.com/robotology/WB-Toolbox#migrating-from-wbi-toolbox) +- [Migrating from WB-Toolbox 2.0](https://github.com/robotology/WB-Toolbox#migrating-from-wbi-toolbox-2.0) - [Modifying the Toolbox](https://github.com/robotology/WB-Toolbox#modifyng-the-toolbox) - [Citing this work](https://github.com/robotology/WB-Toolbox#citing-this-work) -### Installation +## Installation -#### Requirements -* Matlab V. 7.1+ and Simulink (Tested with Matlab R2015b, R2014a/b, R2013a/b, R2012a/b) -* YARP (https://github.com/robotology/yarp) **-IMPORTANT-** Please compile as shared library. Currently a default yarp configuration option. -* (Optional. Needed for some blocks) iCub (https://github.com/robotology/icub-main) -* yarpWholeBodyInterface (https://github.com/robotology/yarp-wholebodyinterface) +### Requirements +* Matlab V. 7.1+ and Simulink (Tested with Matlab `R2015b`, `R2014a/b`, `R2013a/b`, `R2012a/b`) +* [`YARP`](https://github.com/robotology/yarp) (Compiled as **shared library**. Currently a default yarp configuration option) +* [`iDynTree`](https://github.com/robotology/idyntree) +* Supported Operating Systems: **Linux**, **macOS**, **Windows** **Note:** You can install the dependencies either manually or by using the [codyco-superbuild](https://github.com/robotology/codyco-superbuild). -###### Optional +### Optional requirements +* [`iCub`](https://github.com/robotology/icub-main) (needed for some blocks) * Gazebo Simulator (http://gazebosim.org/) * gazebo_yarp_plugins (https://github.com/robotology/gazebo_yarp_plugins). -**Operating Systems supported: macOS, Linux, Windows.** - **Note: The following instructions are for *NIX systems, but it works similarly on the other operating systems.** -#### Compiling the C++ Code (Mex File) +### Compiling the C++ Code (Mex File) + +**Prerequisite: Check the [Matlab configuration](https://github.com/robotology/WB-Toolbox#matlab-configuration) section.** -**Prerequisite: Check the Matlab configuration.** Before going ahead with the compilation of the library, make sure that you have MATLAB and Simulink properly installed and running. Then, check that the MEX compiler for MATLAB is setup and working. For this you can try compiling some of the MATLAB C code examples as described in [http://www.mathworks.com/help/matlab/ref/mex.html#btz1tb5-12]. **If you installed Matlab in a location different from the default one, please set an environmental variable called either `MATLABDIR` or `MATLAB_DIR` with the root of your Matlab installation**, e.g. add a line to your `~/.bashrc` such as: `export MATLAB_DIR=/usr/local/bin/matlab` +Before going ahead with the compilation of the library, make sure that you have MATLAB and Simulink properly installed and running. Then, check that the MEX compiler for MATLAB is setup and working. For this you can try compiling some of the MATLAB C code examples as described in the [mex official documentation](https://www.mathworks.com/help/matlab/ref/mex.html). +**If you installed Matlab in a location different from the default one, please set an environmental variable called either `MATLABDIR` or `MATLAB_DIR` with the root of your Matlab installation**, e.g. add a line to your `~/.bashrc` such as: `export MATLAB_DIR=/usr/local/bin/matlab`. If you used the [codyco-superbuild](https://github.com/robotology/codyco-superbuild) you can skip this step and go directly to the Matlab configuration step. -Otherwise follow the following instructions. -- Clone this repository: `git clone https://github.com/robotology/WB-Toolbox.git` -- Create the build directory: `mkdir -p WB-Toolbox/build` -- Move inside the build directory: `cd WB-Toolbox/build` -- Configure the project: `cmake ..` +Execute: + +```sh +git clone https://github.com/robotology/WB-Toolbox.git +mkdir -p WB-Toolbox/build +cd WB-Toolbox/build +cmake .. +``` + +**Note:** We suggest to install the project instead of using it from the build directory. You should thus change the default installation directory by configuring the `CMAKE_INSTALL_PREFIX` variable. You can do it before running the first `cmake` command by calling `cmake .. -DCMAKE_INSTALL_PREFIX=/your/new/path`, or by configuring the project with `ccmake .` -**Note** We suggest to install the project instead of using it from the build directory. You should thus change the default installation directory by configuring the `CMAKE_INSTALL_PREFIX` variable. You can do it before running the first `cmake` command by calling `cmake .. -DCMAKE_INSTALL_PREFIX=/your/new/path`, or by configuring the project with `ccmake .` +Compile and install the toolbox as follows: -- Compile the project: `make` -- *[Optional/Suggested]* Install the project: `make install` +```sh +make +make install +``` +### Matlab configuration -#### Matlab configuration In the following section we assume the `install` folder is either the one you specified in the previous step by using `CMAKE_INSTALL_PREFIX` or if you used the `codyco-superbuild`, is `/path/to/superbuild/build_folder/install`. In order to use the WB-Toolbox in Matlab you have to add the `mex` and `share/WB-Toolbox` to the Matlab path. @@ -72,10 +81,10 @@ In order to use the WB-Toolbox in Matlab you have to add the `mex` and `share/WB ``` You can also use (only once after the installation) the script `startup_wbitoolbox.m` that you can find in the `share/WB-Toolbox` subdirectory of the install folder to properly configure Matlab. -**Note** This script configures Matlab to alwasy add the WB-Toolbox to the path. This assumes Matlab is always launched from the `userpath` folder. +**Note:** This script configures Matlab to always add the WB-Toolbox to the path. This assumes Matlab is always launched from the `userpath` folder. - **Launching Matlab** By default, Matlab has different startup behaviours depending on the Operating Systems and how it is launched. For Windows and macOS (if launched in the Finder) Matlab starts in the `userpath` folder. If this is your common workflow you can skip the rest of this note. Instead, if you launch Matlab from the command line (Linux and macOS users), Matlab starts in the folder where the command is typed, and thus the `path.m` file generated in the previous phase is no longer loaded. You thus have two options: - - create a Bash alias, such that Matlab is always launched in the `userpath`, e.g. + - create a Bash alias, such that Matlab is always launched in the `userpath`, e.g. ``` alias matlab='cd ~/Documents/MATLAB && /path/to/matlab ``` @@ -90,75 +99,86 @@ You can also use (only once after the installation) the script `startup_wbitoolb - **Robots' configuration files** Each robot that can be used through the Toolbox has its own configuration file. WB-Toolbox uses the Yarp [`ResourceFinder`](http://www.yarp.it/yarp_resource_finder_tutorials.html). You should thus follow the related instruction to properly configure your installation (e.g. set the `YARP_DATA_DIRS` variable) -### Troubleshooting -- **Problems finding libraries and libstdc++.** In case Matlab has trouble finding a specific library, a workaround is to launch it preloading the variable `LD_PRELOAD` (or `DYLD_INSERT_LIBRARIES` on macOS) with the full path of the missing library. On Linux you might also have trouble with libstdc++.so since Matlab comes with its own. To use your system's libstdc++ you would need to launch Matlab something like (replace with your system's libstdc++ library): +## Troubleshooting +- **Problems finding libraries and libstdc++.** In case Matlab has trouble finding a specific library, a workaround is to launch it preloading the variable `LD_PRELOAD` (or `DYLD_INSERT_LIBRARIES` on macOS) with the full path of the missing library. On Linux you might also have trouble with `libstdc++.so` since Matlab comes with its own. To use your system's `libstdc++` you would need to launch Matlab something like (replace with your system's `libstdc++` library): -`LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.19 matlab` +`LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab` You could additionally create an alias to launch Matlab this way: -`alias matlab_codyco="cd ~/Documents/MATLAB && LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.19 matlab"` +`alias matlab_codyco="cd ~/Documents/MATLAB && LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6 matlab"` -A more permanent and scalable solution can be found in the answer of [this issue](https://github.com/robotology/codyco-superbuild/issues/141#issuecomment-257892256) +A more permanent and scalable solution can be found in https://github.com/robotology/codyco-superbuild/issues/141#issuecomment-257892256. + +- In case you compiled `YARP` in a directory different from the system default one and you are not using RPATH, you need to tell to MATLAB the location in which to find the shared libraries for `YARP`. If you launch MATLAB from command line, this task is already done for you by `bash` (if you edited `.bashrc`). If you launch MATLAB from the UI (e.g. on macOS by double clicking the application icon) you need to further add the variables in `${MATLAB_ROOT}/bin/.matlab7rc.sh` by first doing -- In case you have compiled YARP in a directory different from the system default one and you are not using RPATH, you need to tell to MATLAB the location in which to find the shared libraries for YARP. If you launch MATLAB from command line, this task is already done for you by `bash` (if you edited `.bashrc`). If you launch MATLAB from the UI (e.g. on macOS by double clicking the application icon) you need to further add the variables in `${MATLAB_ROOT}/bin/.matlab7rc.sh` by first doing ```bash chmod +w .matlab7rc.sh ``` + Then looking for the variable `LDPATH_SUFFIX` and assign to every instance the contents of your `DYLD_LIBRARY_PATH`. Finally do: + ```bash chmod -w .matlab7rc.sh ``` The error message you get in this case might look something like: + ```bash Library not loaded: libyarpwholeBodyinterface.0.0.1.dylib Referenced from: ${CODYCO_SUPERBUILD_DIR}/install/mex/robotState.mexmaci64 ``` -### Using the Toolbox +## Using the Toolbox + The Toolbox assumes the following information / variables to be defined: -##### Environment variables + +#### Environment variables + If you launch Matlab from command line, it inherits the configuration from the `.bashrc` or `.bash_profile` file. If you launch Matlab directly from the GUI you should define this variables with the Matlab function `setenv`. - `YARP_ROBOT_NAME` - `YARP_DATA_DIRS` -##### Matlab variables - -- `WBT_robotName`: if not defined the robot name defined in the `yarpWholeBodyInterface.ini` file is taken. -- `WBT_modelName`: if not defined by default is `WBT_simulink`. -- `WBT_wbiFilename`: the name of the Yarp WholeBodyInterface file, by default `yarpWholeBodyInterface.ini`. -- `WBT_wbiList`: the name of the robot list to be used in the Toolbox. By default `ROBOT_TORQUE_CONTROL_JOINTS`. +#### Creating a model -**Creating a model** Before using or creating a new model keep in mind that WB-Toolbox is discrete in principle and your simulation should be discrete as well. By going to `Simulation > Configuration Parameters > Solver` you should change the solver options to `Fixed Step` and use a `discrete (no continuous states)` solver. +r +In order to start dragging and dropping blocks from the Toolbox, open the Simulink Library Browser and search for `Whole Body Toolbox` in the tree view. + +## Migrating -To start dragging and dropping blocks from the Toolbox open Simulink and search for `Whole Body Toolbox` in the libraries tree. +#### From WBI-Toolbox (1.0) -### Migrating from WBI-Toolbox -Please see [here](doc/Migration.md). +Please see [here](doc/Migration_from_WBI-Toolbox_1.0.md). + +#### Migrating from WB-Toolbox 2.0 + +Please see [here](doc/Migration_from_WB-Toolbox_2.0.md). + +## Modifying the Toolbox -### Modifying the Toolbox If you want to modify the toolbox, please check developers documentation: - [Add a new C++ block](doc/dev/create_new_block.md) - [Tip and tricks on creating simulink blocks](doc/dev/sim_tricks.md) -### Citing this work +## Citing this work + Please cite the following publication if you are using WB-Toolbox for your own research and/or robot controllers > Romano, F., Traversaro, S., Pucci, D., Del Prete, A., Eljaik, J., Nori, F. "A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems", 1st IEEE International Conference on Robotic Computing, 2017. Bibtex citation: ``` -@INPROCEEDINGS{RomanoWBI17, -author={F. Romano and S. Traversaro and D. Pucci and A. Del Prete and J. Eljaik and F. Nori}, -booktitle={2017 IEEE 1st International Conference on Robotic Computing}, -title={A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems}, -year={2017}, +@INPROCEEDINGS{RomanoWBI17, +author={F. Romano and S. Traversaro and D. Pucci and A. Del Prete and J. Eljaik and F. Nori}, +booktitle={2017 IEEE 1st International Conference on Robotic Computing}, +title={A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems}, +year={2017}, } ``` #### Tested OS -macOS, Linux, Windows + +Linux, macOS, Windows. diff --git a/cmake/FindEigen3.cmake b/cmake/FindEigen3.cmake deleted file mode 100644 index 82846c3ad..000000000 --- a/cmake/FindEigen3.cmake +++ /dev/null @@ -1,90 +0,0 @@ - -# - Try to find Eigen3 lib -# -# This module supports requiring a minimum version, e.g. you can do -# find_package(Eigen3 3.1.2) -# to require version 3.1.2 or newer of Eigen3. -# -# Once done this will define -# -# EIGEN3_FOUND - system has eigen lib with correct version -# EIGEN3_INCLUDE_DIR - the eigen include directory -# EIGEN3_VERSION - eigen version -# -# This module reads hints about search locations from -# the following enviroment variables: -# -# EIGEN3_ROOT -# EIGEN3_ROOT_DIR - -# Copyright (c) 2006, 2007 Montel Laurent, -# Copyright (c) 2008, 2009 Gael Guennebaud, -# Copyright (c) 2009 Benoit Jacob -# Redistribution and use is allowed according to the terms of the 2-clause BSD license. - -if(NOT Eigen3_FIND_VERSION) - if(NOT Eigen3_FIND_VERSION_MAJOR) - set(Eigen3_FIND_VERSION_MAJOR 2) - endif(NOT Eigen3_FIND_VERSION_MAJOR) - if(NOT Eigen3_FIND_VERSION_MINOR) - set(Eigen3_FIND_VERSION_MINOR 91) - endif(NOT Eigen3_FIND_VERSION_MINOR) - if(NOT Eigen3_FIND_VERSION_PATCH) - set(Eigen3_FIND_VERSION_PATCH 0) - endif(NOT Eigen3_FIND_VERSION_PATCH) - - set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") -endif(NOT Eigen3_FIND_VERSION) - -macro(_eigen3_check_version) - file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) - - string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") - set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") - set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") - string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") - set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") - - set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) - if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK FALSE) - else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - set(EIGEN3_VERSION_OK TRUE) - endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) - - if(NOT EIGEN3_VERSION_OK) - - message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " - "but at least version ${Eigen3_FIND_VERSION} is required") - endif(NOT EIGEN3_VERSION_OK) -endmacro(_eigen3_check_version) - -if (EIGEN3_INCLUDE_DIR) - - # in cache already - _eigen3_check_version() - set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) - -else (EIGEN3_INCLUDE_DIR) - - find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library - HINTS - ENV EIGEN3_ROOT - ENV EIGEN3_ROOT_DIR - PATHS - ${CMAKE_INSTALL_PREFIX}/include - ${KDE4_INCLUDE_DIR} - PATH_SUFFIXES eigen3 eigen - ) - - if(EIGEN3_INCLUDE_DIR) - _eigen3_check_version() - endif(EIGEN3_INCLUDE_DIR) - - include(FindPackageHandleStandardArgs) - find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) - - mark_as_advanced(EIGEN3_INCLUDE_DIR) - -endif(EIGEN3_INCLUDE_DIR) diff --git a/cmake/FindMatlab.cmake b/cmake/FindMatlab.cmake deleted file mode 100644 index 373aa38a3..000000000 --- a/cmake/FindMatlab.cmake +++ /dev/null @@ -1,1467 +0,0 @@ -#.rst: -# FindMatlab -# ---------- -# -# Finds Matlab installations and provides Matlab tools and libraries to cmake. -# -# This package first intention is to find the libraries associated with Matlab -# in order to be able to build Matlab extensions (mex files). It can also be -# used: -# -# * run specific commands in Matlab -# * declare Matlab unit test -# * retrieve various information from Matlab (mex extensions, versions and -# release queries, ...) -# -# The module supports the following components: -# -# * ``MX_LIBRARY`` and ``ENG_LIBRARY`` respectively the MX and ENG libraries of -# Matlab -# * ``MAIN_PROGRAM`` the Matlab binary program. -# -# .. note:: -# -# The version given to the :command:`find_package` directive is the Matlab -# **version**, which should not be confused with the Matlab *release* name -# (eg. `R2014`). -# The :command:`matlab_get_version_from_release_name` and -# :command:`matlab_get_release_name_from_version` allow a mapping -# from the release name to the version. -# -# The variable :variable:`Matlab_ROOT_DIR` may be specified in order to give -# the path of the desired Matlab version. Otherwise, the behaviour is platform -# specific: -# -# * Windows: The installed versions of Matlab are retrieved from the -# Windows registry -# * OS X: The installed versions of Matlab are given by the MATLAB -# paths in ``/Application``. If no such application is found, it falls back -# to the one that might be accessible from the PATH. -# * Unix: The desired Matlab should be accessible from the PATH. -# -# Additional information is provided when :variable:`MATLAB_FIND_DEBUG` is set. -# When a Matlab binary is found automatically and the ``MATLAB_VERSION`` -# is not given, the version is queried from Matlab directly. -# On Windows, it can make a window running Matlab appear. -# -# The mapping of the release names and the version of Matlab is performed by -# defining pairs (name, version). The variable -# :variable:`MATLAB_ADDITIONAL_VERSIONS` may be provided before the call to -# the :command:`find_package` in order to handle additional versions. -# -# A Matlab scripts can be added to the set of tests using the -# :command:`matlab_add_unit_test`. By default, the Matlab unit test framework -# will be used (>= 2013a) to run this script, but regular ``.m`` files -# returning an exit code can be used as well (0 indicating a success). -# -# Module Input Variables -# ^^^^^^^^^^^^^^^^^^^^^^ -# -# Users or projects may set the following variables to configure the module -# behaviour: -# -# :variable:`Matlab_ROOT_DIR` -# the root of the Matlab installation. -# :variable:`MATLAB_FIND_DEBUG` -# outputs debug information -# :variable:`MATLAB_ADDITIONAL_VERSIONS` -# additional versions of Matlab for the automatic retrieval of the installed -# versions. -# -# Variables defined by the module -# ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -# -# Result variables -# """""""""""""""" -# -# ``Matlab_FOUND`` -# ``TRUE`` if the Matlab installation is found, ``FALSE`` -# otherwise. All variable below are defined if Matlab is found. -# ``Matlab_ROOT_DIR`` -# the final root of the Matlab installation determined by the FindMatlab -# module. -# ``Matlab_MAIN_PROGRAM`` -# the Matlab binary program. Available only if the component ``MAIN_PROGRAM`` -# is given in the :command:`find_package` directive. -# ``Matlab_INCLUDE_DIRS`` -# the path of the Matlab libraries headers -# ``Matlab_MEX_LIBRARY`` -# library for mex, always available. -# ``Matlab_MX_LIBRARY`` -# mx library of Matlab (arrays). Available only if the component -# ``MX_LIBRARY`` has been requested. -# ``Matlab_ENG_LIBRARY`` -# Matlab engine library. Available only if the component ``ENG_LIBRARY`` -# is requested. -# ``Matlab_LIBRARIES`` -# the whole set of libraries of Matlab -# ``Matlab_MEX_COMPILER`` -# the mex compiler of Matlab. Currently not used. -# Available only if the component ``MEX_COMPILER`` is asked -# -# Cached variables -# """""""""""""""" -# -# ``Matlab_MEX_EXTENSION`` -# the extension of the mex files for the current platform (given by Matlab). -# ``Matlab_ROOT_DIR`` -# the location of the root of the Matlab installation found. If this value -# is changed by the user, the result variables are recomputed. -# -# Provided macros -# ^^^^^^^^^^^^^^^ -# -# :command:`matlab_get_version_from_release_name` -# returns the version from the release name -# :command:`matlab_get_release_name_from_version` -# returns the release name from the Matlab version -# -# Provided functions -# ^^^^^^^^^^^^^^^^^^ -# -# :command:`matlab_add_mex` -# adds a target compiling a MEX file. -# :command:`matlab_add_unit_test` -# adds a Matlab unit test file as a test to the project. -# :command:`matlab_extract_all_installed_versions_from_registry` -# parses the registry for all Matlab versions. Available on Windows only. -# The part of the registry parsed is dependent on the host processor -# :command:`matlab_get_all_valid_matlab_roots_from_registry` -# returns all the possible Matlab paths, according to a previously -# given list. Only the existing/accessible paths are kept. This is mainly -# useful for the searching all possible Matlab installation. -# :command:`matlab_get_mex_suffix` -# returns the suffix to be used for the mex files -# (platform/architecture dependant) -# :command:`matlab_get_version_from_matlab_run` -# returns the version of Matlab, given the full directory of the Matlab -# program. -# -# -# Known issues -# ^^^^^^^^^^^^ -# -# **Symbol clash in a MEX target** -# By default, every symbols inside a MEX -# file defined with the command :command:`matlab_add_mex` have hidden -# visibility, except for the entry point. This is the default behaviour of -# the MEX compiler, which lowers the risk of symbol collision between the -# libraries shipped with Matlab, and the libraries to which the MEX file is -# linking to. This is also the default on Windows platforms. -# -# However, this is not sufficient in certain case, where for instance your -# MEX file is linking against libraries that are already loaded by Matlab, -# even if those libraries have different SONAMES. -# A possible solution is to hide the symbols of the libraries to which the -# MEX target is linking to. This can be achieved in GNU GCC compilers with -# the linker option ``-Wl,--exclude-libs,ALL``. -# -# **Tests using GPU resources** -# in case your MEX file is using the GPU and -# in order to be able to run unit tests on this MEX file, the GPU resources -# should be properly released by Matlab. A possible solution is to make -# Matlab aware of the use of the GPU resources in the session, which can be -# performed by a command such as ``D = gpuDevice()`` at the beginning of -# the test script (or via a fixture). -# -# -# Reference -# ^^^^^^^^^ -# -# .. variable:: Matlab_ROOT_DIR -# -# The root folder of the Matlab installation. If set before the call to -# :command:`find_package`, the module will look for the components in that -# path. If not set, then an automatic search of Matlab -# will be performed. If set, it should point to a valid version of Matlab. -# -# .. variable:: MATLAB_FIND_DEBUG -# -# If set, the lookup of Matlab and the intermediate configuration steps are -# outputted to the console. -# -# .. variable:: MATLAB_ADDITIONAL_VERSIONS -# -# If set, specifies additional versions of Matlab that may be looked for. -# The variable should be a list of strings, organised by pairs of release -# name and versions, such as follows:: -# -# set(MATLAB_ADDITIONAL_VERSIONS -# "release_name1=corresponding_version1" -# "release_name2=corresponding_version2" -# ... -# ) -# -# Example:: -# -# set(MATLAB_ADDITIONAL_VERSIONS -# "R2013b=8.2" -# "R2013a=8.1" -# "R2012b=8.0") -# -# The order of entries in this list matters when several versions of -# Matlab are installed. The priority is set according to the ordering in -# this list. - -#============================================================================= -# Copyright 2014-2015 Raffi Enficiaud, Max Planck Society -# -# Distributed under the OSI-approved BSD License (the "License"); -# see accompanying file Copyright.txt for details. -# -# This software is distributed WITHOUT ANY WARRANTY; without even the -# implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. -# See the License for more information. -#============================================================================= -# (To distribute this file outside of CMake, substitute the full -# License text for the above reference.) - -set(_FindMatlab_SELF_DIR "${CMAKE_CURRENT_LIST_DIR}") - -include(FindPackageHandleStandardArgs) -include(CheckCXXCompilerFlag) - - -# The currently supported versions. Other version can be added by the user by -# providing MATLAB_ADDITIONAL_VERSIONS -if(NOT MATLAB_ADDITIONAL_VERSIONS) - set(MATLAB_ADDITIONAL_VERSIONS) -endif() - -set(MATLAB_VERSIONS_MAPPING - "R2016b=9.1" - "R2016a=9.0" - "R2015b=8.6" - "R2015a=8.5" - "R2014b=8.4" - "R2014a=8.3" - "R2013b=8.2" - "R2013a=8.1" - "R2012b=8.0" - "R2012a=7.14" - - "R2011b=7.13" - "R2011a=7.12" - "R2010b=7.11" - - ${MATLAB_ADDITIONAL_VERSIONS} - ) - - -# temporary folder for all Matlab runs -set(_matlab_temporary_folder ${CMAKE_BINARY_DIR}/Matlab) - -if(NOT EXISTS "${_matlab_temporary_folder}") - file(MAKE_DIRECTORY "${_matlab_temporary_folder}") -endif() - -#.rst: -# .. command:: matlab_get_version_from_release_name -# -# Returns the version of Matlab (17.58) from a release name (R2017k) -macro (matlab_get_version_from_release_name release_name version_name) - - string(REGEX MATCHALL "${release_name}=([0-9]+\\.?[0-9]*)" _matched ${MATLAB_VERSIONS_MAPPING}) - - set(${version_name} "") - if(NOT _matched STREQUAL "") - set(${version_name} ${CMAKE_MATCH_1}) - else() - message(WARNING "The release name ${release_name} is not registered") - endif() - unset(_matched) - -endmacro() - - - - - -#.rst: -# .. command:: matlab_get_release_name_from_version -# -# Returns the release name (R2017k) from the version of Matlab (17.58) -macro (matlab_get_release_name_from_version version release_name) - - set(${release_name} "") - foreach(_var IN LISTS MATLAB_VERSIONS_MAPPING) - string(REGEX MATCHALL "(.+)=${version}" _matched ${_var}) - if(NOT _matched STREQUAL "") - set(${release_name} ${CMAKE_MATCH_1}) - break() - endif() - endforeach(_var) - - unset(_var) - unset(_matched) - if(${release_name} STREQUAL "") - message(WARNING "The version ${version} is not registered") - endif() - -endmacro() - - - - - -# extracts all the supported release names (R2017k...) of Matlab -# internal use -macro(matlab_get_supported_releases list_releases) - set(${list_releases}) - foreach(_var IN LISTS MATLAB_VERSIONS_MAPPING) - string(REGEX MATCHALL "(.+)=([0-9]+\\.?[0-9]*)" _matched ${_var}) - if(NOT _matched STREQUAL "") - list(APPEND ${list_releases} ${CMAKE_MATCH_1}) - endif() - unset(_matched) - unset(CMAKE_MATCH_1) - endforeach(_var) - unset(_var) -endmacro() - - - -# extracts all the supported versions of Matlab -# internal use -macro(matlab_get_supported_versions list_versions) - set(${list_versions}) - foreach(_var IN LISTS MATLAB_VERSIONS_MAPPING) - string(REGEX MATCHALL "(.+)=([0-9]+\\.?[0-9]*)" _matched ${_var}) - if(NOT _matched STREQUAL "") - list(APPEND ${list_versions} ${CMAKE_MATCH_2}) - endif() - unset(_matched) - unset(CMAKE_MATCH_1) - endforeach(_var) - unset(_var) -endmacro() - - -#.rst: -# .. command:: matlab_extract_all_installed_versions_from_registry -# -# This function parses the registry and founds the Matlab versions that are -# installed. The found versions are returned in `matlab_versions`. -# Set `win64` to `TRUE` if the 64 bit version of Matlab should be looked for -# The returned list contains all versions under -# ``HKLM\\SOFTWARE\\Mathworks\\MATLAB`` or an empty list in case an error -# occurred (or nothing found). -# -# .. note:: -# -# Only the versions are provided. No check is made over the existence of the -# installation referenced in the registry, -# -function(matlab_extract_all_installed_versions_from_registry win64 matlab_versions) - - if(NOT CMAKE_HOST_WIN32) - message(FATAL_ERROR "This macro can only be called by a windows host (call to reg.exe") - endif() - - - if(${win64} AND ${CMAKE_HOST_SYSTEM_PROCESSOR} MATCHES "64") - set(APPEND_REG "/reg:64") - else() - set(APPEND_REG "/reg:32") - endif() - - # /reg:64 should be added on 64 bits capable OSs in order to enable the - # redirection of 64 bits applications - execute_process( - COMMAND reg query HKEY_LOCAL_MACHINE\\SOFTWARE\\Mathworks\\MATLAB /f * /k ${APPEND_REG} - RESULT_VARIABLE resultMatlab - OUTPUT_VARIABLE varMatlab - ERROR_VARIABLE errMatlab - INPUT_FILE NUL - ) - - - set(matlabs_from_registry) - if(${resultMatlab} EQUAL 0) - - string( - REGEX MATCHALL "MATLAB\\\\([0-9]+(\\.[0-9]+)?)" - matlab_versions_regex ${varMatlab}) - - foreach(match IN LISTS matlab_versions_regex) - string( - REGEX MATCH "MATLAB\\\\(([0-9]+)(\\.([0-9]+))?)" - current_match ${match}) - - set(_matlab_current_version ${CMAKE_MATCH_1}) - set(current_matlab_version_major ${CMAKE_MATCH_2}) - set(current_matlab_version_minor ${CMAKE_MATCH_4}) - if(NOT current_matlab_version_minor) - set(current_matlab_version_minor "0") - endif() - - list(APPEND matlabs_from_registry ${_matlab_current_version}) - unset(_matlab_current_version) - endforeach(match) - - endif() - - if(matlabs_from_registry) - list(REMOVE_DUPLICATES matlabs_from_registry) - list(SORT matlabs_from_registry) - list(REVERSE matlabs_from_registry) - endif() - - set(${matlab_versions} ${matlabs_from_registry} PARENT_SCOPE) - -endfunction() - - - -# (internal) -macro(extract_matlab_versions_from_registry_brute_force matlab_versions) - # get the supported versions - set(matlab_supported_versions) - matlab_get_supported_versions(matlab_supported_versions) - - - # this is a manual population of the versions we want to look for - # this can be done as is, but preferably with the call to - # matlab_get_supported_versions and variable - - # populating the versions we want to look for - # set(matlab_supported_versions) - - # # Matlab 7 - # set(matlab_major 7) - # foreach(current_matlab_minor RANGE 4 20) - # list(APPEND matlab_supported_versions "${matlab_major}.${current_matlab_minor}") - # endforeach(current_matlab_minor) - - # # Matlab 8 - # set(matlab_major 8) - # foreach(current_matlab_minor RANGE 0 5) - # list(APPEND matlab_supported_versions "${matlab_major}.${current_matlab_minor}") - # endforeach(current_matlab_minor) - - # # taking into account the possible additional versions provided by the user - # if(DEFINED MATLAB_ADDITIONAL_VERSIONS) - # list(APPEND matlab_supported_versions MATLAB_ADDITIONAL_VERSIONS) - # endif() - - - # we order from more recent to older - if(matlab_supported_versions) - list(REMOVE_DUPLICATES matlab_supported_versions) - list(SORT matlab_supported_versions) - list(REVERSE matlab_supported_versions) - endif() - - - set(${matlab_versions} ${matlab_supported_versions}) - - -endmacro() - - - - -#.rst: -# .. command:: matlab_get_all_valid_matlab_roots_from_registry -# -# Populates the Matlab root with valid versions of Matlab. -# The returned matlab_roots is organized in pairs -# ``(version_number,matlab_root_path)``. -# -# :: -# -# matlab_get_all_valid_matlab_roots_from_registry( -# matlab_versions -# matlab_roots) -# -# ``matlab_versions`` -# the versions of each of the Matlab installations -# ``matlab_roots`` -# the location of each of the Matlab installations -function(matlab_get_all_valid_matlab_roots_from_registry matlab_versions matlab_roots) - - # The matlab_versions comes either from - # extract_matlab_versions_from_registry_brute_force or - # matlab_extract_all_installed_versions_from_registry. - - - set(_matlab_roots_list ) - foreach(_matlab_current_version ${matlab_versions}) - get_filename_component( - current_MATLAB_ROOT - "[HKEY_LOCAL_MACHINE\\SOFTWARE\\MathWorks\\MATLAB\\${_matlab_current_version};MATLABROOT]" - ABSOLUTE) - - if(EXISTS ${current_MATLAB_ROOT}) - list(APPEND _matlab_roots_list ${_matlab_current_version} ${current_MATLAB_ROOT}) - endif() - - endforeach(_matlab_current_version) - unset(_matlab_current_version) - set(${matlab_roots} ${_matlab_roots_list} PARENT_SCOPE) - unset(_matlab_roots_list) -endfunction() - -#.rst: -# .. command:: matlab_get_mex_suffix -# -# Returns the extension of the mex files (the suffixes). -# This function should not be called before the appropriate Matlab root has -# been found. -# -# :: -# -# matlab_get_mex_suffix( -# matlab_root -# mex_suffix) -# -# ``matlab_root`` -# the root of the Matlab installation -# ``mex_suffix`` -# the variable name in which the suffix will be returned. -function(matlab_get_mex_suffix matlab_root mex_suffix) - - # todo setup the extension properly. Currently I do not know if this is - # sufficient for all win32 distributions. - # there is also CMAKE_EXECUTABLE_SUFFIX that could be tweaked - set(mexext_suffix "") - if(WIN32) - list(APPEND mexext_suffix ".bat") - endif() - - # we first try without suffix, since cmake does not understand a list with - # one empty string element - find_program( - Matlab_MEXEXTENSIONS_PROG - NAMES mexext - PATHS ${matlab_root}/bin - DOC "Matlab MEX extension provider" - NO_DEFAULT_PATH - ) - - foreach(current_mexext_suffix IN LISTS mexext_suffix) - if(NOT DEFINED Matlab_MEXEXTENSIONS_PROG OR NOT Matlab_MEXEXTENSIONS_PROG) - # this call should populate the cache automatically - find_program( - Matlab_MEXEXTENSIONS_PROG - "mexext${current_mexext_suffix}" - PATHS ${matlab_root}/bin - DOC "Matlab MEX extension provider" - NO_DEFAULT_PATH - ) - endif() - endforeach(current_mexext_suffix) - - - # the program has been found? - if((NOT Matlab_MEXEXTENSIONS_PROG) OR (NOT EXISTS ${Matlab_MEXEXTENSIONS_PROG})) - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Cannot found mexext program. Matlab root is ${matlab_root}") - endif() - unset(Matlab_MEXEXTENSIONS_PROG CACHE) - return() - endif() - - set(_matlab_mex_extension) - - set(devnull) - if(UNIX) - set(devnull INPUT_FILE /dev/null) - elseif(WIN32) - set(devnull INPUT_FILE NUL) - endif() - - execute_process( - COMMAND ${Matlab_MEXEXTENSIONS_PROG} - OUTPUT_VARIABLE _matlab_mex_extension - ERROR_VARIABLE _matlab_mex_extension_error - ${devnull}) - string(STRIP ${_matlab_mex_extension} _matlab_mex_extension) - - unset(Matlab_MEXEXTENSIONS_PROG CACHE) - set(${mex_suffix} ${_matlab_mex_extension} PARENT_SCOPE) -endfunction() - - - - -#.rst: -# .. command:: matlab_get_version_from_matlab_run -# -# This function runs Matlab program specified on arguments and extracts its -# version. -# -# :: -# -# matlab_get_version_from_matlab_run( -# matlab_binary_path -# matlab_list_versions) -# -# ``matlab_binary_path`` -# the location of the `matlab` binary executable -# ``matlab_list_versions`` -# the version extracted from Matlab -function(matlab_get_version_from_matlab_run matlab_binary_program matlab_list_versions) - - set(${matlab_list_versions} "" PARENT_SCOPE) - - - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Determining the version of Matlab from ${matlab_binary_program}") - endif() - - if(EXISTS "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Removing previous ${_matlab_temporary_folder}/matlabVersionLog.cmaketmp file") - endif() - file(REMOVE "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - endif() - - - # the log file is needed since on windows the command executes in a new - # window and it is not possible to get back the answer of Matlab - # the -wait command is needed on windows, otherwise the call returns - # immediately after the program launches itself. - if(WIN32) - set(_matlab_additional_commands "-wait") - endif() - - set(devnull) - if(UNIX) - set(devnull INPUT_FILE /dev/null) - elseif(WIN32) - set(devnull INPUT_FILE NUL) - endif() - - # timeout set to 30 seconds, in case it does not start - # note as said before OUTPUT_VARIABLE cannot be used in a platform - # independent manner however, not setting it would flush the output of Matlab - # in the current console (unix variant) - execute_process( - COMMAND "${matlab_binary_program}" -nosplash -nojvm ${_matlab_additional_commands} -logfile "matlabVersionLog.cmaketmp" -nodesktop -nodisplay -r "version, exit" - OUTPUT_VARIABLE _matlab_version_from_cmd_dummy - RESULT_VARIABLE _matlab_result_version_call - ERROR_VARIABLE _matlab_result_version_call_error - TIMEOUT 30 - WORKING_DIRECTORY "${_matlab_temporary_folder}" - ${devnull} - ) - - - if(${_matlab_result_version_call}) - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Unable to determine the version of Matlab. Matlab call returned with error ${_matlab_result_version_call}.") - endif() - return() - elseif(NOT EXISTS "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Unable to determine the version of Matlab. The log file does not exist.") - endif() - return() - endif() - - # if successful, read back the log - file(READ "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp" _matlab_version_from_cmd) - file(REMOVE "${_matlab_temporary_folder}/matlabVersionLog.cmaketmp") - - set(index -1) - string(FIND ${_matlab_version_from_cmd} "ans" index) - if(index EQUAL -1) - - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Cannot find the version of Matlab returned by the run.") - endif() - - else() - set(matlab_list_of_all_versions_tmp) - - string(SUBSTRING ${_matlab_version_from_cmd} ${index} -1 substring_ans) - string( - REGEX MATCHALL "ans[\r\n\t ]*=[\r\n\t ]*([0-9]+(\\.[0-9]+)?)" - matlab_versions_regex - ${substring_ans}) - foreach(match IN LISTS matlab_versions_regex) - string( - REGEX MATCH "ans[\r\n\t ]*=[\r\n\t ]*(([0-9]+)(\\.([0-9]+))?)" - current_match ${match}) - - list(APPEND matlab_list_of_all_versions_tmp ${CMAKE_MATCH_1}) - endforeach() - if(matlab_list_of_all_versions_tmp) - list(REMOVE_DUPLICATES matlab_list_of_all_versions_tmp) - endif() - set(${matlab_list_versions} ${matlab_list_of_all_versions_tmp} PARENT_SCOPE) - - endif() - -endfunction() - - -#.rst: -# .. command:: matlab_add_unit_test -# -# Adds a Matlab unit test to the test set of cmake/ctest. -# This command requires the component ``MAIN_PROGRAM``. -# The unit test uses the Matlab unittest framework (default, available -# starting Matlab 2013b+) except if the option ``NO_UNITTEST_FRAMEWORK`` -# is given. -# -# The function expects one Matlab test script file to be given. -# In the case ``NO_UNITTEST_FRAMEWORK`` is given, the unittest script file -# should contain the script to be run, plus an exit command with the exit -# value. This exit value will be passed to the ctest framework (0 success, -# non 0 failure). Additional arguments accepted by :command:`add_test` can be -# passed through ``TEST_ARGS`` (eg. ``CONFIGURATION ...``). -# -# :: -# -# matlab_add_unit_test( -# NAME -# UNITTEST_FILE matlab_file_containing_unittest.m -# [UNITTEST_PRECOMMAND matlab_command_to_run] -# [TIMEOUT timeout] -# [ADDITIONAL_PATH path1 [path2 ...]] -# [MATLAB_ADDITIONAL_STARTUP_OPTIONS option1 [option2 ...]] -# [TEST_ARGS arg1 [arg2 ...]] -# [NO_UNITTEST_FRAMEWORK] -# ) -# -# The function arguments are: -# -# ``NAME`` -# name of the unittest in ctest. -# ``UNITTEST_FILE`` -# the matlab unittest file. Its path will be automatically -# added to the Matlab path. -# ``UNITTEST_PRECOMMAND`` -# Matlab script command to be ran before the file -# containing the test (eg. GPU device initialisation based on CMake -# variables). -# ``TIMEOUT`` -# the test timeout in seconds. Defaults to 180 seconds as the -# Matlab unit test may hang. -# ``ADDITIONAL_PATH`` -# a list of paths to add to the Matlab path prior to -# running the unit test. -# ``MATLAB_ADDITIONAL_STARTUP_OPTIONS`` -# a list of additional option in order -# to run Matlab from the command line. -# ``TEST_ARGS`` -# Additional options provided to the add_test command. These -# options are added to the default options (eg. "CONFIGURATIONS Release") -# ``NO_UNITTEST_FRAMEWORK`` -# when set, indicates that the test should not -# use the unittest framework of Matlab (available for versions >= R2013a). -# -function(matlab_add_unit_test) - - if(NOT Matlab_MAIN_PROGRAM) - message(FATAL_ERROR "[MATLAB] This functionality needs the MAIN_PROGRAM component (not default)") - endif() - - set(options NO_UNITTEST_FRAMEWORK) - set(oneValueArgs NAME UNITTEST_PRECOMMAND UNITTEST_FILE TIMEOUT) - set(multiValueArgs ADDITIONAL_PATH MATLAB_ADDITIONAL_STARTUP_OPTIONS TEST_ARGS) - - set(prefix _matlab_unittest_prefix) - cmake_parse_arguments(${prefix} "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} ) - - if(NOT ${prefix}_NAME) - message(FATAL_ERROR "[MATLAB] The Matlab test name cannot be empty") - endif() - - add_test(NAME ${${prefix}_NAME} - COMMAND ${CMAKE_COMMAND} - -Dtest_name=${${prefix}_NAME} - -Dadditional_paths=${${prefix}_ADDITIONAL_PATH} - -Dtest_timeout=${${prefix}_TIMEOUT} - -Doutput_directory=${_matlab_temporary_folder} - -DMatlab_PROGRAM=${Matlab_MAIN_PROGRAM} - -Dno_unittest_framework=${${prefix}_NO_UNITTEST_FRAMEWORK} - -DMatlab_ADDITIONNAL_STARTUP_OPTIONS=${${prefix}_MATLAB_ADDITIONAL_STARTUP_OPTIONS} - -Dunittest_file_to_run=${${prefix}_UNITTEST_FILE} - -Dcmd_to_run_before_test=${${prefix}_UNITTEST_PRECOMMAND} - -P ${_FindMatlab_SELF_DIR}/MatlabTestsRedirect.cmake - ${${prefix}_TEST_ARGS} - ${${prefix}_UNPARSED_ARGUMENTS} - ) -endfunction() - - -#.rst: -# .. command:: matlab_add_mex -# -# Adds a Matlab MEX target. -# This commands compiles the given sources with the current tool-chain in -# order to produce a MEX file. The final name of the produced output may be -# specified, as well as additional link libraries, and a documentation entry -# for the MEX file. Remaining arguments of the call are passed to the -# :command:`add_library` command. -# -# :: -# -# matlab_add_mex( -# NAME -# SRC src1 [src2 ...] -# [OUTPUT_NAME output_name] -# [DOCUMENTATION file.txt] -# [LINK_TO target1 target2 ...] -# [...] -# ) -# -# ``NAME`` -# name of the target. -# ``SRC`` -# list of tje source files. -# ``LINK_TO`` -# a list of additional link dependencies. The target links to ``libmex`` -# by default. If ``Matlab_MX_LIBRARY`` is defined, it also -# links to ``libmx``. -# ``OUTPUT_NAME`` -# if given, overrides the default name. The default name is -# the name of the target without any prefix and -# with ``Matlab_MEX_EXTENSION`` suffix. -# ``DOCUMENTATION`` -# if given, the file ``file.txt`` will be considered as -# being the documentation file for the MEX file. This file is copied into -# the same folder without any processing, with the same name as the final -# mex file, and with extension `.m`. In that case, typing ``help `` -# in Matlab prints the documentation contained in this file. -# -# The documentation file is not processed and should be in the following -# format: -# -# :: -# -# % This is the documentation -# function ret = mex_target_output_name(input1) -# -function(matlab_add_mex ) - - if(NOT WIN32) - # we do not need all this on Windows - # pthread options - check_cxx_compiler_flag(-pthread HAS_MINUS_PTHREAD) - # we should use try_compile instead, the link flags are discarded from - # this compiler_flag function. - #check_cxx_compiler_flag(-Wl,--exclude-libs,ALL HAS_SYMBOL_HIDING_CAPABILITY) - - endif() - - set(oneValueArgs NAME DOCUMENTATION OUTPUT_NAME) - set(multiValueArgs LINK_TO SRC) - - set(prefix _matlab_addmex_prefix) - cmake_parse_arguments(${prefix} "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN} ) - - if(NOT ${prefix}_NAME) - message(FATAL_ERROR "[MATLAB] The MEX target name cannot be empty") - endif() - - if(NOT ${prefix}_OUTPUT_NAME) - set(${prefix}_OUTPUT_NAME ${${prefix}_NAME}) - endif() - - add_library(${${prefix}_NAME} - SHARED - ${${prefix}_SRC} - ${${prefix}_DOCUMENTATION} - ${${prefix}_UNPARSED_ARGUMENTS}) - target_include_directories(${${prefix}_NAME} PRIVATE ${Matlab_INCLUDE_DIRS}) - - if(DEFINED Matlab_MX_LIBRARY) - target_link_libraries(${${prefix}_NAME} ${Matlab_MX_LIBRARY}) - endif() - - target_link_libraries(${${prefix}_NAME} ${Matlab_MEX_LIBRARY} ${${prefix}_LINK_TO}) - set_target_properties(${${prefix}_NAME} - PROPERTIES - PREFIX "" - OUTPUT_NAME ${${prefix}_OUTPUT_NAME} - SUFFIX ".${Matlab_MEX_EXTENSION}") - - - # documentation - if(NOT ${${prefix}_DOCUMENTATION} STREQUAL "") - get_target_property(output_name ${${prefix}_NAME} OUTPUT_NAME) - add_custom_command( - TARGET ${${prefix}_NAME} - PRE_BUILD - COMMAND ${CMAKE_COMMAND} -E copy_if_different ${${prefix}_DOCUMENTATION} $/${output_name}.m - COMMENT "Copy ${${prefix}_NAME} documentation file into the output folder" - ) - endif() # documentation - - # entry point in the mex file + taking care of visibility and symbol clashes. - if(WIN32) - set_target_properties(${${prefix}_NAME} - PROPERTIES - DEFINE_SYMBOL "DLL_EXPORT_SYM=__declspec(dllexport)") - else() - - if(HAS_MINUS_PTHREAD AND NOT APPLE) - # Apparently, compiling with -pthread generated the proper link flags - # and some defines at compilation - target_compile_options(${${prefix}_NAME} PRIVATE "-pthread") - endif() - - - # if we do not do that, the symbols linked from eg. boost remain weak and - # then clash with the ones defined in the matlab process. So by default - # the symbols are hidden. - # This also means that for shared libraries (like MEX), the entry point - # should be explicitly declared with default visibility, otherwise Matlab - # cannot find the entry point. - # Note that this is particularly meaningful if the MEX wrapper itself - # contains symbols that are clashing with Matlab (that are compiled in the - # MEX file). In order to propagate the visibility options to the libraries - # to which the MEX file is linked against, the -Wl,--exclude-libs,ALL - # option should also be specified. - - set_target_properties(${${prefix}_NAME} - PROPERTIES - CXX_VISIBILITY_PRESET "hidden" - C_VISIBILITY_PRESET "hidden" - VISIBILITY_INLINES_HIDDEN ON - ) - - # get_target_property( - # _previous_link_flags - # ${${prefix}_NAME} - # LINK_FLAGS) - # if(NOT _previous_link_flags) - # set(_previous_link_flags) - # endif() - - # if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "GNU") - # set_target_properties(${${prefix}_NAME} - # PROPERTIES - # LINK_FLAGS "${_previous_link_flags} -Wl,--exclude-libs,ALL" - # # -Wl,--version-script=${_FindMatlab_SELF_DIR}/MatlabLinuxVisibility.map" - # ) - # elseif("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") - # # in this case, all other symbols become hidden. - # set_target_properties(${${prefix}_NAME} - # PROPERTIES - # LINK_FLAGS "${_previous_link_flags} -Wl,-exported_symbol,_mexFunction" - # #-Wl,-exported_symbols_list,${_FindMatlab_SELF_DIR}/MatlabOSXVisilibity.map" - # ) - # endif() - - - - set_target_properties(${${prefix}_NAME} - PROPERTIES - DEFINE_SYMBOL "DLL_EXPORT_SYM=__attribute__ ((visibility (\"default\")))" - ) - - - endif() - -endfunction() - - -# (internal) -# Used to get the version of matlab, using caching. This basically transforms the -# output of the root list, with possible unknown version, to a version -# -function(_Matlab_get_version_from_root matlab_root matlab_known_version matlab_final_version) - - # if the version is not trivial, we query matlab for that - # we keep track of the location of matlab that induced this version - #if(NOT DEFINED Matlab_PROG_VERSION_STRING_AUTO_DETECT) - # set(Matlab_PROG_VERSION_STRING_AUTO_DETECT "" CACHE INTERNAL "internal matlab location for the discovered version") - #endif() - - if(NOT ${matlab_known_version} STREQUAL "NOTFOUND") - # the version is known, we just return it - set(${matlab_final_version} ${matlab_known_version} PARENT_SCOPE) - set(Matlab_VERSION_STRING_INTERNAL ${matlab_known_version} CACHE INTERNAL "Matlab version (automatically determined)" FORCE) - return() - endif() - - # - set(_matlab_current_program ${Matlab_MAIN_PROGRAM}) - - # do we already have a matlab program? - if(NOT _matlab_current_program) - - set(_find_matlab_options) - if(matlab_root AND EXISTS ${matlab_root}) - set(_find_matlab_options PATHS ${matlab_root} ${matlab_root}/bin NO_DEFAULT_PATH) - endif() - - find_program( - _matlab_current_program - matlab - ${_find_matlab_options} - DOC "Matlab main program" - ) - endif() - - if(NOT _matlab_current_program OR NOT EXISTS ${_matlab_current_program}) - # if not found, clear the dependent variables - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Cannot find the main matlab program under ${matlab_root}") - endif() - set(Matlab_PROG_VERSION_STRING_AUTO_DETECT "" CACHE INTERNAL "internal matlab location for the discovered version" FORCE) - set(Matlab_VERSION_STRING_INTERNAL "" CACHE INTERNAL "internal matlab location for the discovered version" FORCE) - unset(_matlab_current_program) - unset(_matlab_current_program CACHE) - return() - endif() - - # full real path for path comparison - get_filename_component(_matlab_main_real_path_tmp "${_matlab_current_program}" REALPATH) - unset(_matlab_current_program) - unset(_matlab_current_program CACHE) - - # is it the same as the previous one? - if(_matlab_main_real_path_tmp STREQUAL Matlab_PROG_VERSION_STRING_AUTO_DETECT) - set(${matlab_final_version} ${Matlab_VERSION_STRING_INTERNAL} PARENT_SCOPE) - return() - endif() - - # update the location of the program - set(Matlab_PROG_VERSION_STRING_AUTO_DETECT ${_matlab_main_real_path_tmp} CACHE INTERNAL "internal matlab location for the discovered version" FORCE) - - set(matlab_list_of_all_versions) - matlab_get_version_from_matlab_run("${Matlab_PROG_VERSION_STRING_AUTO_DETECT}" matlab_list_of_all_versions) - - list(GET matlab_list_of_all_versions 0 _matlab_version_tmp) - - # set the version into the cache - set(Matlab_VERSION_STRING_INTERNAL ${_matlab_version_tmp} CACHE INTERNAL "Matlab version (automatically determined)" FORCE) - - # warning, just in case several versions found (should not happen) - list(LENGTH matlab_list_of_all_versions list_of_all_versions_length) - if((${list_of_all_versions_length} GREATER 1) AND MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Found several versions, taking the first one (versions found ${matlab_list_of_all_versions})") - endif() - - # return the updated value - set(${matlab_final_version} ${Matlab_VERSION_STRING_INTERNAL} PARENT_SCOPE) - -endfunction() - - - - - - - -# ################################### -# Exploring the possible Matlab_ROOTS - -# this variable will get all Matlab installations found in the current system. -set(_matlab_possible_roots) - - - -if(Matlab_ROOT_DIR) - # if the user specifies a possible root, we keep this one - - if(NOT EXISTS ${Matlab_ROOT_DIR}) - # if Matlab_ROOT_DIR specified but erroneous - if(MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] the specified path for Matlab_ROOT_DIR does not exist (${Matlab_ROOT_DIR})") - endif() - else() - # NOTFOUND indicates the code below to search for the version automatically - if("${Matlab_VERSION_STRING_INTERNAL}" STREQUAL "") - list(APPEND _matlab_possible_roots "NOTFOUND" ${Matlab_ROOT_DIR}) # empty version - else() - list(APPEND _matlab_possible_roots ${Matlab_VERSION_STRING_INTERNAL} ${Matlab_ROOT_DIR}) # cached version - endif() - endif() - - -else() - - # if the user does not specify the possible installation root, we look for - # one installation using the appropriate heuristics - - if(WIN32) - - # On WIN32, we look for Matlab installation in the registry - # if unsuccessful, we look for all known revision and filter the existing - # ones. - - # testing if we are able to extract the needed information from the registry - set(_matlab_versions_from_registry) - matlab_extract_all_installed_versions_from_registry(CMAKE_CL_64 _matlab_versions_from_registry) - - # the returned list is empty, doing the search on all known versions - if(NOT _matlab_versions_from_registry) - - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Search for Matlab from the registry unsuccessful, testing all supported versions") - endif() - - extract_matlab_versions_from_registry_brute_force(_matlab_versions_from_registry) - endif() - - # filtering the results with the registry keys - matlab_get_all_valid_matlab_roots_from_registry("${_matlab_versions_from_registry}" _matlab_possible_roots) - unset(_matlab_versions_from_registry) - - elseif(APPLE) - - # on mac, we look for the /Application paths - # this corresponds to the behaviour on Windows. On Linux, we do not have - # any other guess. - matlab_get_supported_releases(_matlab_releases) - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Matlab supported versions ${_matlab_releases}. If more version should be supported " - "the variable MATLAB_ADDITIONAL_VERSIONS can be set according to the documentation") - endif() - - foreach(_matlab_current_release IN LISTS _matlab_releases) - set(_matlab_full_string "/Applications/MATLAB_${_matlab_current_release}.app") - if(EXISTS ${_matlab_full_string}) - set(_matlab_current_version) - matlab_get_version_from_release_name("${_matlab_current_release}" _matlab_current_version) - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Found version ${_matlab_current_release} (${_matlab_current_version}) in ${_matlab_full_string}") - endif() - list(APPEND _matlab_possible_roots ${_matlab_current_version} ${_matlab_full_string}) - unset(_matlab_current_version) - endif() - - unset(_matlab_full_string) - endforeach(_matlab_current_release) - - unset(_matlab_current_release) - unset(_matlab_releases) - - endif() - -endif() - - - -list(LENGTH _matlab_possible_roots _numbers_of_matlab_roots) -if(_numbers_of_matlab_roots EQUAL 0) - # if we have not found anything, we fall back on the PATH - - - # At this point, we have no other choice than trying to find it from PATH. - # If set by the user, this wont change - find_program( - _matlab_main_tmp - NAMES matlab) - - - if(_matlab_main_tmp) - # we then populate the list of roots, with empty version - if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] matlab found from PATH: ${_matlab_main_tmp}") - endif() - - # resolve symlinks - get_filename_component(_matlab_current_location "${_matlab_main_tmp}" REALPATH) - - # get the directory (the command below has to be run twice) - # this will be the matlab root - get_filename_component(_matlab_current_location "${_matlab_current_location}" DIRECTORY) - get_filename_component(_matlab_current_location "${_matlab_current_location}" DIRECTORY) # Matlab should be in bin - - list(APPEND _matlab_possible_roots "NOTFOUND" ${_matlab_current_location}) - - unset(_matlab_current_location) - - endif() - unset(_matlab_main_tmp CACHE) - -endif() - - - - - -if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Matlab root folders are ${_matlab_possible_roots}") -endif() - - - - - -# take the first possible Matlab root -list(LENGTH _matlab_possible_roots _numbers_of_matlab_roots) -set(Matlab_VERSION_STRING "NOTFOUND") -if(_numbers_of_matlab_roots GREATER 0) - list(GET _matlab_possible_roots 0 Matlab_VERSION_STRING) - list(GET _matlab_possible_roots 1 Matlab_ROOT_DIR) - - # adding a warning in case of ambiguity - if(_numbers_of_matlab_roots GREATER 2 AND MATLAB_FIND_DEBUG) - message(WARNING "[MATLAB] Found several distributions of Matlab. Setting the current version to ${Matlab_VERSION_STRING} (located ${Matlab_ROOT_DIR})." - " If this is not the desired behaviour, provide the -DMatlab_ROOT_DIR=... on the command line") - endif() -endif() - - -# check if the root changed against the previous defined one, if so -# clear all the cached variables -if(DEFINED Matlab_ROOT_DIR_LAST_CACHED) - - if(NOT Matlab_ROOT_DIR_LAST_CACHED STREQUAL Matlab_ROOT_DIR) - set(_Matlab_cached_vars - Matlab_INCLUDE_DIRS - Matlab_MEX_LIBRARY - Matlab_MEX_COMPILER - Matlab_MAIN_PROGRAM - Matlab_MX_LIBRARY - Matlab_ENG_LIBRARY - Matlab_MEX_EXTENSION - - # internal - Matlab_MEXEXTENSIONS_PROG - Matlab_ROOT_DIR_LAST_CACHED - #Matlab_PROG_VERSION_STRING_AUTO_DETECT - Matlab_VERSION_STRING_INTERNAL - ) - foreach(_var IN LISTS _Matlab_cached_vars) - if(DEFINED ${_var}) - unset(${_var} CACHE) - endif() - endforeach() - endif() -endif() - -set(Matlab_ROOT_DIR_LAST_CACHED ${Matlab_ROOT_DIR} CACHE INTERNAL "last Matlab root dir location") -set(Matlab_ROOT_DIR ${Matlab_ROOT_DIR} CACHE PATH "Matlab installation root path" FORCE) - -# Fix the version, in case this one is NOTFOUND -_Matlab_get_version_from_root( - "${Matlab_ROOT_DIR}" - ${Matlab_VERSION_STRING} - Matlab_VERSION_STRING -) - - - - -if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] Current version is ${Matlab_VERSION_STRING} located ${Matlab_ROOT_DIR}") -endif() - - - -if(Matlab_ROOT_DIR) - file(TO_CMAKE_PATH ${Matlab_ROOT_DIR} Matlab_ROOT_DIR) -endif() - -if(CMAKE_SIZEOF_VOID_P EQUAL 4) - set(_matlab_64Build FALSE) -else() - set(_matlab_64Build TRUE) -endif() - -if(APPLE) - set(_matlab_bin_prefix "mac") # i should be for intel - set(_matlab_bin_suffix_32bits "i") - set(_matlab_bin_suffix_64bits "i64") -elseif(UNIX) - set(_matlab_bin_prefix "gln") - set(_matlab_bin_suffix_32bits "x86") - set(_matlab_bin_suffix_64bits "xa64") -else() - set(_matlab_bin_prefix "win") - set(_matlab_bin_suffix_32bits "32") - set(_matlab_bin_suffix_64bits "64") -endif() - - - -set(MATLAB_INCLUDE_DIR_TO_LOOK ${Matlab_ROOT_DIR}/extern/include) -if(_matlab_64Build) - set(_matlab_current_suffix ${_matlab_bin_suffix_64bits}) -else() - set(_matlab_current_suffix ${_matlab_bin_suffix_32bits}) -endif() - -set(Matlab_BINARIES_DIR - ${Matlab_ROOT_DIR}/bin/${_matlab_bin_prefix}${_matlab_current_suffix}) -set(Matlab_EXTERN_LIBRARY_DIR - ${Matlab_ROOT_DIR}/extern/lib/${_matlab_bin_prefix}${_matlab_current_suffix}) - -if(WIN32) - set(_matlab_lib_dir_for_search ${Matlab_EXTERN_LIBRARY_DIR}/microsoft) - set(_matlab_lib_prefix_for_search "lib") -else() - set(_matlab_lib_dir_for_search ${Matlab_BINARIES_DIR}) - set(_matlab_lib_prefix_for_search "lib") -endif() - -unset(_matlab_64Build) - - -if(NOT DEFINED Matlab_MEX_EXTENSION) - set(_matlab_mex_extension "") - matlab_get_mex_suffix("${Matlab_ROOT_DIR}" _matlab_mex_extension) - - # This variable goes to the cache. - set(Matlab_MEX_EXTENSION ${_matlab_mex_extension} CACHE STRING "Extensions for the mex targets (automatically given by Matlab)") - unset(_matlab_mex_extension) -endif() - - -if(MATLAB_FIND_DEBUG) - message(STATUS "[MATLAB] [DEBUG]_matlab_lib_prefix_for_search = ${_matlab_lib_prefix_for_search} | _matlab_lib_dir_for_search = ${_matlab_lib_dir_for_search}") -endif() - - - -# internal -# This small stub around find_library is to prevent any pollution of CMAKE_FIND_LIBRARY_PREFIXES in the global scope. -# This is the function to be used below instead of the find_library directives. -function(_Matlab_find_library _matlab_library_prefix) - set(CMAKE_FIND_LIBRARY_PREFIXES ${CMAKE_FIND_LIBRARY_PREFIXES} ${_matlab_library_prefix}) - find_library(${ARGN}) -endfunction() - - -set(_matlab_required_variables) - - -# the MEX library/header are required -find_path( - Matlab_INCLUDE_DIRS - mex.h - PATHS ${MATLAB_INCLUDE_DIR_TO_LOOK} - NO_DEFAULT_PATH - ) -list(APPEND _matlab_required_variables Matlab_INCLUDE_DIRS) - -_Matlab_find_library( - ${_matlab_lib_prefix_for_search} - Matlab_MEX_LIBRARY - mex - PATHS ${_matlab_lib_dir_for_search} - NO_DEFAULT_PATH -) - - -list(APPEND _matlab_required_variables Matlab_MEX_LIBRARY) - -# the MEX extension is required -list(APPEND _matlab_required_variables Matlab_MEX_EXTENSION) - -# the matlab root is required -list(APPEND _matlab_required_variables Matlab_ROOT_DIR) - - -# component Mex Compiler -list(FIND Matlab_FIND_COMPONENTS MEX_COMPILER _matlab_find_mex_compiler) -if(_matlab_find_mex_compiler GREATER -1) - find_program( - Matlab_MEX_COMPILER - "mex" - PATHS ${Matlab_BINARIES_DIR} - DOC "Matlab MEX compiler" - NO_DEFAULT_PATH - ) - - if(Matlab_MEX_COMPILER) - set(Matlab_MEX_COMPILER_FOUND TRUE) - endif() -endif() -unset(_matlab_find_mex_compiler) - -# component Matlab program -list(FIND Matlab_FIND_COMPONENTS MAIN_PROGRAM _matlab_find_matlab_program) -if(_matlab_find_matlab_program GREATER -1) - - find_program( - Matlab_MAIN_PROGRAM - matlab - PATHS ${Matlab_ROOT_DIR} ${Matlab_ROOT_DIR}/bin - DOC "Matlab main program" - NO_DEFAULT_PATH - ) - - if(Matlab_MAIN_PROGRAM) - set(Matlab_MAIN_PROGRAM_FOUND TRUE) - endif() - -endif() -unset(_matlab_find_matlab_program) - -# Component MX library -list(FIND Matlab_FIND_COMPONENTS MX_LIBRARY _matlab_find_mx) -if(_matlab_find_mx GREATER -1) - _Matlab_find_library( - ${_matlab_lib_prefix_for_search} - Matlab_MX_LIBRARY - mx - PATHS ${_matlab_lib_dir_for_search} - NO_DEFAULT_PATH - ) - - if(Matlab_MX_LIBRARY) - set(Matlab_MX_LIBRARY_FOUND TRUE) - endif() -endif() -unset(_matlab_find_mx) - - -# Component ENG library -list(FIND Matlab_FIND_COMPONENTS ENG_LIBRARY _matlab_find_eng) -if(_matlab_find_eng GREATER -1) - _Matlab_find_library( - ${_matlab_lib_prefix_for_search} - Matlab_ENG_LIBRARY - eng - PATHS ${_matlab_lib_dir_for_search} - NO_DEFAULT_PATH - ) - if(Matlab_ENG_LIBRARY) - set(Matlab_ENG_LIBRARY_FOUND TRUE) - endif() -endif() -unset(_matlab_find_eng) - - - - - -unset(_matlab_lib_dir_for_search) - - -set(Matlab_LIBRARIES ${Matlab_MEX_LIBRARY} ${Matlab_MX_LIBRARY} ${Matlab_ENG_LIBRARY}) - - -find_package_handle_standard_args( - Matlab - FOUND_VAR Matlab_FOUND - REQUIRED_VARS ${_matlab_required_variables} - VERSION_VAR Matlab_VERSION_STRING - HANDLE_COMPONENTS) - -unset(_matlab_required_variables) -unset(_matlab_bin_prefix) -unset(_matlab_bin_suffix_32bits) -unset(_matlab_bin_suffix_64bits) -unset(_matlab_current_suffix) -unset(_matlab_lib_dir_for_search) -unset(_matlab_lib_prefix_for_search) - -if(Matlab_INCLUDE_DIRS AND Matlab_LIBRARIES) - mark_as_advanced( - #Matlab_LIBRARIES - Matlab_MEX_LIBRARY - Matlab_MX_LIBRARY - Matlab_ENG_LIBRARY - Matlab_INCLUDE_DIRS - Matlab_FOUND - #Matlab_ROOT_DIR - #Matlab_VERSION_STRING - Matlab_MAIN_PROGRAM - #Matlab_MEX_EXTENSION - Matlab_MEXEXTENSIONS_PROG - Matlab_MEX_EXTENSION - #Matlab_BINARIES_DIR - ) -endif() diff --git a/doc/Migration_from_WB-Toolbox_2.0.md b/doc/Migration_from_WB-Toolbox_2.0.md new file mode 100644 index 000000000..b194665b9 --- /dev/null +++ b/doc/Migration_from_WB-Toolbox_2.0.md @@ -0,0 +1,82 @@ +# Migration from WB-Toolbox 2.0 to WB-Toolbox 3.0 + +Most of the major changes delivered with the `3.0` version of the `WB-Toolbox` don't affect directly the end-user. Under the hood the toolbox had an important polishing, and the small manual intervention required by this new release match the new features which have been developed. + +You can read [Release Notes](https://github.com/robotology/WB-Toolbox/issues/65) for a detailed overview. Below are described only the steps required to port Simulink models to this new release. + +## New toolbox configuration + +The `WB-Toolbox 2.0` was based on top of [`yarpWholeBodyInterface`](https://github.com/robotology/yarp-wholebodyinterface), which configuration was stored in a `yarpWholeBodyInterface.ini` file. This file was retrieved by `ResourceFinder` and its information was then loaded into the toolbox. + +### Store the configuration in the Simulink model + +`WB-Toolbox 3.0` deprecated the support of `yarpWholeBodyInterface`, and for reducing the complexity and sparsity of the information storage it allows configuring a Simulink model from the model itself. + +The new _Configuration_ block allows inserting information such as **Robot Name**, **URDF Name**, **Controlled Joints**, ... directly from the block's mask. + +### Load the configuration from the Workspace + +Sometimes it might be useful loading the model's configuration directly from the Workspace. For this purpose, a new `WBToolbox.WBToolboxConfig` class has been developed. The _Configuration_ block needs to know only the name of the variable which refers to the object. Its data is then read before the simulation runs. + +This snippet of code shows an example of how to initialize a configuration object: + +```matlab +# Initialize a config object +WBTConfigRobot = WBToolbox.WBToolboxConfig; + +# Insert robot data +WBTConfigRobot.RobotName = 'icubSim'; +WBTConfigRobot.UrdfFile = 'model.urdf'; +WBTConfigRobot.ControlledJoints = {... + 'torso_pitch','torso_roll','torso_yaw',... + 'r_shoulder_pitch','r_shoulder_roll','r_shoulder_yaw','r_elbow',... + 'l_shoulder_pitch','l_shoulder_roll','l_shoulder_yaw','l_elbow'}; +WBTConfigRobot.ControlBoardsNames = {'torso','left_arm','right_arm'}; +WBTConfigRobot.LocalName = 'WBT'; +``` + +To check if the data has been read correctly, it is displayed as read-only in the block's mask. + +Furthermore, a good sign for a valid configuration is the `WBTConfigRobot.ValidConfiguration` property. + +### Multi-robot support + +The scope of the introduction of the _Configuration_ block goes beyond the need of a simpler toolbox configuration. One of the biggest limitation of the `2.0` version is the support of [controlling only one robot per model](https://github.com/robotology/WB-Toolbox/issues/37). + +`WB-Toolbox 3.0` is now capable of reading / sending data from / to multiple robots. Multiple _Configuration_ blocks can be present in the same model attaining to the following rules: + +* In the same hierarchical level of a Simulink model, only one _Configuration_ + block should be present. In other words, you should never see in the display more than one _Configuration_ block. + * _Configuration_ blocks put deeper in the hierarchy (e.g. in a Subsystem) override the previous ones. + + There are a few pitfalls which are worth to be highlighted: + + * It is legit having two Subsystems with different _Configuration_ blocks which point to the same robot. They can have for instance a different joint list and use different control boards. Although, despite reading information never creates problems, sending data to the robot in such scenario can be disastrous. In fact, consider the case these two subsystems share one link, and configure it in two different control modes (e.g. Position and Torque). Sending references to this link causes unpredictable effects. + * In line of theory it would be possible to have two subsystems in which the first one refers to a Gazebo model and the second one to a real robot. However, this case causes unpredictable behaviour for what concerns the synchronization. In fact, two different blocks for such aim are present in the toolbox: _Simulator Synchronizer_ and _Real Time Syncronizer_. They should be always used exclusively. + +## Other manual edits + +* All the _Get Estimate_ blocks need to be replaced by the new _Get Measurement_ block. +* All the hardcoded digital filters (e.g. for the joints velocities) have been removed. A new `Discrete Filter` block has been developed, and it should be manually added if the read raw signal (e.g. from the _Get Measurement_ block) requires filtering. +* The `C++` class used by the _DoFs Converter_ changed. All the blocks in the `YARP To WBI` configuration need to be connected again. +* The gravity vector is stored is the `WBToolboxConfig` class. If an alternative value is needed, set it globally directly in the configuration object or scope the block which needs it in a Subsystem with its own _Configuration_ block. +* In order to set the low level PIDs, loading in the Workspace a `WBToolbox.WBTPIDConfig` object should be configured as follows: + +```matlab +# Initialize an empty object +pids = WBToolbox.WBTPIDConfig; + +# Insert data +pids.addPID(WBToolbox.PID('l_elbow', WBToolbox.PID(1, 1, 0))); +pids.addPID(WBToolbox.PID('l_wrist_pitch', WBToolbox.PID(1.5, 0, 0.1))); +pids.addPID(WBToolbox.PID('r_shoulder_pitch', WBToolbox.PID(0.2, 0, 0))); +pids.addPID(WBToolbox.PID('torso_roll', WBToolbox.PID(0.1, 0.1, 0))); +``` + +If some of the controlled joints are not specified, the PIDs are kept in their default values. + + +## Deprecations + +* _Inverse Kinematics_ and _Remote Inverse Kinematics_ have been temporary deprecated. They will see a major release in the coming months. If you need them please do not upgrade to the `3.0` version. +* _Set Low Level PID_ block lost the capability of switching between multiple configurations. Since they were stored in an external file, this change is aligned to the simplification process chosen for for the configuration. \ No newline at end of file diff --git a/doc/Migration.md b/doc/Migration_from_WBI-Toolbox_1.0.md similarity index 96% rename from doc/Migration.md rename to doc/Migration_from_WBI-Toolbox_1.0.md index b73a4590b..a89c8c6b7 100644 --- a/doc/Migration.md +++ b/doc/Migration_from_WBI-Toolbox_1.0.md @@ -1,4 +1,4 @@ -## Migration from WB**I**-Toolbox to WB-Toolbox 2.0 +## Migration from WBI-Toolbox to WB-Toolbox 2.0 Given a simulink model with some WBI-Toolbox blocks inside, the general procedure is to **substitute each block with the corresponding one from WB-Toolbox 2.0**. However, there are some things the user should take care while doing this operation. This guide points out the main differences between the two toolboxes. For more information about the WBI-Toolbox, please have a look at the [WBI-Toolbox README](https://github.com/robotology-playground/WBI-Toolbox/blob/master/README.md). This guide follows the WBI and WB Toolbox blocks partitioning in Simulink library. It is divided in the following sections: @@ -24,7 +24,7 @@ They have already meaningful default values. Nevertheless you should take a look The world-to-base homogeneous transformation matrix is a 4x4 matrix that maps position and orientation of a rigid body from an initial frame of reference to another. -For back-compatibility, the transformation happending under the hood in the WBI-Toolbox can be obtained using forward kinematics blocks as in the following example: +For back-compatibility, the transformation happending under the hood in the WBI-Toolbox can be obtained using forward kinematics blocks as in the following example: ![](https://cloud.githubusercontent.com/assets/12396934/12293044/3337da3c-b9f1-11e5-959f-b40418b5469d.png) @@ -43,7 +43,7 @@ It is divided into three subsections. The `Joint Limits` block is now moved into #### Dynamics - the `dJdq` blocks have been moved into **jacobians** subsection; -- for mass matrix, generalized bias forces and centroidal momentum computation is now required to calculate explicitly the world-to-base homogeneous transformation matrix and the base velocity. Furthermore, the base frame pose and velocity and the joint configuration are now separate inputs. +- for mass matrix, generalized bias forces and centroidal momentum computation is now required to calculate explicitly the world-to-base homogeneous transformation matrix and the base velocity. Furthermore, the base frame pose and velocity and the joint configuration are now separate inputs. #### Jacobians There is now only one generic block for jacobians and one for `dJdq` calculation. The link with respect to which the Jacobian is computed is determined by its frame name as specified in the **URDF model**. As for the dynamics, the base pose and velocity and the joint position and velocity are required as input. diff --git a/tests/models/DiscreteFilter2014b.mdl b/tests/models/DiscreteFilter2014b.mdl new file mode 100644 index 000000000..d328aa757 --- /dev/null +++ b/tests/models/DiscreteFilter2014b.mdl @@ -0,0 +1,1086 @@ +Model { + Name "DiscreteFilter2014b" + Version 8.4 + SavedCharacterEncoding "US-ASCII" + GraphicalInterface { + NumRootInports 0 + NumRootOutports 0 + ParameterArgumentNames "" + ComputedModelVersion "1.64" + NumModelReferences 0 + NumTestPointedSignals 0 + } + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + MinMaxOverflowArchiveMode "Overwrite" + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [151.0, 32.0, 1185.0, 779.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [9] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Object { + $PropName "EditorsInfo" + $ObjectID 5 + $ClassName "Simulink.EditorInfo" + IsActive [1] + ViewObjType "SimulinkTopLevel" + LoadSaveID "0" + Extents [1131.0, 586.0] + ZoomFactor [2.1817619879155012] + Offset [138.17691806436989, 8.7048744900272084] + } + } + } + Created "Tue Aug 08 11:48:28 2017" + Creator "icub" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "icub" + ModifiedDateFormat "%" + LastModifiedDate "Fri Aug 11 14:16:58 2017" + RTWModifiedTimeStamp 424361818 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "disabled" + WideLines on + ShowLineDimensions on + ShowPortDataTypes off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder on + ExecutionContextIcon off + ShowLinearizationAnnotations on + ShowMarkup on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + BrowserLookUnderMasks off + SimulationMode "normal" + PauseTimes "5" + NumberOfSteps 1 + SnapshotBufferSize 10 + SnapshotInterval 10 + NumberOfLastSnapshots 0 + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + AccelSystemTargetFile "accel.tlc" + AccelTemplateMakefile "accel_default_tmf" + AccelMakeCommand "make_rtw" + TryForcingSFcnDF off + Object { + $PropName "DataLoggingOverride" + $ObjectID 7 + $ClassName "Simulink.SimulationData.ModelLoggingInfo" + model_ "DiscreteFilter2014b" + overrideMode_ [0.0] + Array { + Type "Cell" + Dimension 1 + Cell "DiscreteFilter2014b" + PropName "logAsSpecifiedByModels_" + } + Array { + Type "Cell" + Dimension 1 + Cell [] + PropName "logAsSpecifiedByModelsSSIDs_" + } + } + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovMetricSettings "dwe" + CovNameIncrementing off + CovHtmlReporting off + CovForceBlockReductionOff on + CovEnableCumulative on + covSaveCumulativeToWorkspaceVar off + CovSaveSingleToWorkspaceVar off + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovReportOnPause on + CovModelRefEnable "Off" + CovExternalEMLEnable on + CovSFcnEnable on + CovBoundaryAbsTol 0.000010 + CovBoundaryRelTol 0.010000 + CovUseTimeInterval off + CovStartTime 0 + CovStopTime 0 + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 8 + Version "1.14.3" + Array { + Type "Handle" + Dimension 8 + Simulink.SolverCC { + $ObjectID 9 + Version "1.14.3" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "0.01" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + SolverMode "SingleTasking" + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode3" + SolverName "ode3" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + } + Simulink.DataIOCC { + $ObjectID 10 + Version "1.14.3" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints off + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "StructureWithTime" + SignalLoggingSaveFormat "Dataset" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + VisualizeSimOutput on + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + } + Simulink.OptimizationCC { + $ObjectID 11 + Version "1.14.3" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + InlineParams off + UseDivisionForNetSlopeComputation "Off" + UseFloatMulNetSlope off + DefaultUnderspecifiedDataType "double" + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + CachingGlobalReferences off + GlobalBufferReuse on + StrengthReduction off + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + ExpressionDepthLimit 128 + LocalBlockOutputs on + RollThreshold 5 + StateBitsets off + DataBitsets off + ActiveStateOutputEnumStorageType "Native Integer" + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + OptimizeModelRefInitCode off + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "Off" + AccelVerboseBuild off + } + Simulink.DebuggingCC { + $ObjectID 12 + Version "1.14.3" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Simplified" + MergeDetectMultiDrivingBlocksExec "error" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "none" + InheritedTsInSrcMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "EnableAllAsError" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "error" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "none" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "error" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "error" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnconditionalTransitionShadowingDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + IntegerSaturationMsg "warning" + ModelReferenceCSMismatchMessage "none" + } + Simulink.HardwareCC { + $ObjectID 13 + Version "1.14.3" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerLongLong 64 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 64 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "Float" + ProdIntDivRoundTo "Zero" + ProdEndianess "LittleEndian" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdLongLongMode off + ProdHWDeviceType "Specified" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerLongLong 64 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetLongLongMode off + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + } + Simulink.ModelReferenceCC { + $ObjectID 14 + Version "1.14.3" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel on + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 15 + Version "1.14.3" + SFSimEcho on + SimCtrlC on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + SimGenImportedTypeDefs off + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 16 + Version "1.14.3" + Array { + Type "Cell" + Dimension 13 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + Cell "PortableWordSizes" + Cell "GenerateWebview" + Cell "GenerateCodeMetricsReport" + Cell "GenerateCodeReplacementReport" + Cell "GenerateMissedCodeReplacementReport" + Cell "GenerateErtSFunction" + Cell "CreateSILPILBlock" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + TLCOptions "" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + TemplateMakefile "grt_default_tmf" + PostCodeGenCommand "" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + Toolchain "Automatically locate an installed toolchain" + BuildConfiguration "Faster Builds" + IncludeHyperlinkInReport off + LaunchReport off + PortableWordSizes off + CreateSILPILBlock "None" + CodeExecutionProfiling off + CodeExecutionProfileVariable "executionProfile" + CodeProfilingSaveOptions "SummaryOnly" + CodeProfilingInstrumentation off + SILDebugging off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + RTWCompilerOptimization "Off" + RTWCustomCompilerOptimizations "" + CheckMdlBeforeBuild "Off" + SharedConstantsCachingThreshold 1024 + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 17 + Version "1.14.3" + Array { + Type "Cell" + Dimension 25 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "InsertPolySpaceComments" + Cell "SFDataObjDesc" + Cell "MATLABFcnDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InternalIdentifier" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrModelFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "CustomSymbolStrUtil" + Cell "CustomUserTokenString" + Cell "ReqsInCode" + PropName "DisabledProps" + } + ForceParamTrailComments off + GenerateComments on + CommentStyle "Auto" + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + OperatorAnnotations off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M_T" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + CustomSymbolStrUtil "$N$C" + DefineNamingRule "None" + ParamNamingRule "None" + SignalNamingRule "None" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifier "Shortened" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 18 + Version "1.14.3" + Array { + Type "Cell" + Dimension 15 + Cell "IncludeMdlTerminateFcn" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "ExistingSharedCode" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "GenerateAllocFcn" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "SupportNonInlinedSFcns" + Cell "RemoveDisableFunc" + Cell "RemoveResetFunc" + PropName "DisabledProps" + } + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + GenFloatMathFcnCalls "NOT IN USE" + TargetLangStandard "C99 (ISO)" + CodeReplacementLibrary "None" + UtilityFuncGeneration "Auto" + ERTMultiwordTypeDef "System defined" + ERTMultiwordLength 256 + MultiwordLength 2048 + GenerateFullHeader on + GenerateSampleERTMain off + GenerateTestInterfaces off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Use local settings" + CombineOutputUpdateFcns on + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + CodeInterfacePackaging "Nonreusable function" + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + SupportVariableSizeSignals off + ParenthesesLevel "Nominal" + CastingMode "Nominal" + MATLABClassNameForMDSCustomization "Simulink.SoftwareTarget.GRTCustomization" + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant on + AutosarCompliant off + GRTInterface off + GenerateAllocFcn off + GenerateSharedConstants on + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + MultiInstanceErrorCode "Error" + } + PropName "Components" + } + } + PropName "Components" + } + Name "Configuration" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 376, 270, 1266, 1010 ] + } + PropName "ConfigurationSets" + } + Simulink.ConfigSet { + $PropName "ActiveConfigurationSet" + $ObjectID 8 + } + Object { + $PropName "DataTransfer" + $ObjectID 20 + $ClassName "Simulink.GlobalDataTransfer" + DefaultTransitionBetweenSyncTasks "Ensure deterministic transfer (maximum delay)" + DefaultTransitionBetweenAsyncTasks "Ensure data integrity only" + DefaultTransitionBetweenContTasks "Ensure deterministic transfer (minimum delay)" + DefaultExtrapolationMethodBetweenContTasks "None" + AutoInsertRateTranBlk [0] + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + MaskDefaults { + SelfModifiable "off" + IconFrame "on" + IconOpaque "on" + RunInitForIconRedraw "off" + IconRotate "none" + PortRotate "default" + IconUnits "autoscale" + } + MaskParameterDefaults { + Evaluate "on" + Tunable "on" + NeverSave "off" + Internal "off" + ReadOnly "off" + Enabled "on" + Visible "on" + ToolTip "on" + } + BlockParameterDefaults { + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Mux + Inputs "4" + DisplayOption "none" + UseBusObject off + BusObject "BusObject" + NonVirtualBus off + } + Block { + BlockType Scope + Floating off + } + Block { + BlockType Sin + SineType "Time based" + TimeSource "Use simulation time" + Amplitude "1" + Bias "0" + Frequency "1" + Phase "0" + Samples "10" + Offset "0" + SampleTime "-1" + VectorParams1D on + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + } + System { + Name "DiscreteFilter2014b" + Location [151, 32, 1336, 811] + Open off + ModelBrowserVisibility off + 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 "218" + ReportName "simulink-default.rpt" + SIDHighWatermark "51" + Block { + BlockType Constant + Name "Constant" + SID "16" + Position [155, 190, 185, 220] + ZOrder 14 + } + Block { + BlockType Reference + Name "DiscreteFilter1" + SID "47" + Ports [1, 1] + Position [440, 110, 500, 140] + ZOrder 99 + BackgroundColor "yellow" + LibraryVersion "1.395" + SourceBlock "WBToolboxLibrary/Utilities/DiscreteFilter" + SourceType "Discrete Filter" + filterType "FirstOrderLowPassFilter" + numCoeffs "[0]" + denCoeffs "[0]" + Fc "1" + Ts "0.01" + orderMedianFilter "0" + initStatus on + y0 "[0 1]" + u0 "[0]" + Port { + PortNumber 1 + Name "filtered_signals" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Mux + Name "Mux" + SID "23" + Ports [2, 1] + Position [395, 106, 400, 144] + ZOrder 20 + ShowName off + Inputs "2" + DisplayOption "bar" + } + Block { + BlockType Mux + Name "Mux1" + SID "50" + Ports [3, 1] + Position [590, 50, 595, 140] + ZOrder 102 + ShowName off + Inputs "3" + DisplayOption "bar" + } + Block { + BlockType Scope + Name "Scope" + SID "51" + Ports [1] + Position [620, 79, 650, 111] + ZOrder 103 + NumInputPorts "1" + Open off + TimeRange auto + TickLabels OneTimeTick + ShowLegends on + LimitDataPoints off + MaxDataPoints 5000 + SaveToWorkspace off + SaveName ScopeData + YMin -1.6103 + YMax 2.6103 + SampleInput off + SampleTime -1 + ZoomMode on + Grid on + DataFormat StructureWithTime + Decimation 1 + List { + ListType AxesTitles + axes1 "%" + } + List { + ListType ScopeGraphics + FigureColor "[0.156862745098039 0.156862745098039 0.156862745098039]" + AxesColor "[0 0 0]" + AxesTickColor "[0.686274509803922 0.686274509803922 0.686274509803922]" + LineColors "[1 1 0;1 0 1;0 1 1;1 0 0;0 1 0;0 0 1]" + LineStyles "-|-|-|-|-|-" + LineWidths "[0.5 0.5 0.5 0.5 0.5 0.5]" + MarkerStyles "none|none|none|none|none|none" + } + Location [675 361 1245 811] + } + Block { + BlockType Sin + Name "Sine 0.5Hz" + SID "6" + Ports [0, 1] + Position [155, 70, 185, 100] + ZOrder 6 + Frequency "2*pi*0.5" + SampleTime "0" + } + Block { + BlockType Sin + Name "Sine 10Hz" + SID "13" + Ports [0, 1] + Position [155, 130, 185, 160] + ZOrder 11 + Amplitude "0.2" + Frequency "2*pi*10" + SampleTime "0" + } + Block { + BlockType Sum + Name "Sum" + SID "12" + Ports [2, 1] + Position [235, 105, 255, 125] + ZOrder 10 + ShowName off + IconShape "round" + Inputs "+|+" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "signal1" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Block { + BlockType Sum + Name "Sum1" + SID "15" + Ports [2, 1] + Position [295, 125, 315, 145] + ZOrder 13 + ShowName off + IconShape "round" + Inputs "|++" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + Port { + PortNumber 1 + Name "signal2" + RTWStorageClass "Auto" + DataLoggingNameMode "SignalName" + } + } + Line { + Name "filtered_signals" + ZOrder 258 + Labels [-1, 1] + SrcBlock "DiscreteFilter1" + SrcPort 1 + DstBlock "Mux1" + DstPort 3 + } + Line { + Name "signal1" + ZOrder 19 + SrcBlock "Sum" + SrcPort 1 + Points [12, 0] + Branch { + ZOrder 73 + Points [47, 0] + Branch { + ZOrder 257 + Labels [-1, 1] + Points [0, -50] + DstBlock "Mux1" + DstPort 1 + } + Branch { + ZOrder 70 + Labels [-1, 1] + DstBlock "Mux" + DstPort 1 + } + } + Branch { + ZOrder 32 + Points [0, 20] + DstBlock "Sum1" + DstPort 1 + } + } + Line { + ZOrder 20 + SrcBlock "Sine 0.5Hz" + SrcPort 1 + Points [55, 0] + DstBlock "Sum" + DstPort 1 + } + Line { + ZOrder 21 + SrcBlock "Sine 10Hz" + SrcPort 1 + Points [55, 0] + DstBlock "Sum" + DstPort 2 + } + Line { + Name "signal2" + ZOrder 41 + SrcBlock "Sum1" + SrcPort 1 + Points [16, 0] + Branch { + ZOrder 256 + Labels [-1, 1] + Points [0, -40] + DstBlock "Mux1" + DstPort 2 + } + Branch { + ZOrder 71 + Labels [-1, 1] + DstBlock "Mux" + DstPort 2 + } + } + Line { + ZOrder 33 + SrcBlock "Constant" + SrcPort 1 + Points [115, 0] + DstBlock "Sum1" + DstPort 2 + } + Line { + ZOrder 227 + SrcBlock "Mux" + SrcPort 1 + DstBlock "DiscreteFilter1" + DstPort 1 + } + Line { + ZOrder 259 + SrcBlock "Mux1" + SrcPort 1 + DstBlock "Scope" + DstPort 1 + } + } +} diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index e648f87f6..fa353fa7a 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -1,5 +1,5 @@ -# Copyright (C) 2013-2015 Instituto Italiano di Tecnologia -# Author: Jorhabib Eljaik, Francesco Romano +# Copyright (C) 2013-2017 iCub Facility - Istituto Italiano di Tecnologia +# Author: Jorhabib Eljaik, Francesco Romano, Diego Ferigo # CopyPolicy: Released under the terms of the GNU GPL v2.0. find_package(YARP REQUIRED) @@ -14,19 +14,17 @@ set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${YARP_MODULE_PATH}) include(YarpInstallationHelpers) yarp_configure_external_installation(codyco) -find_package(Matlab REQUIRED - MX_LIBRARY - ENG_LIBRARY - MAIN_PROGRAM) - +find_package(Matlab REQUIRED MX_LIBRARY + ENG_LIBRARY + MAIN_PROGRAM) if(NOT Matlab_FOUND) message(FATAL_ERROR "Matlab not found") endif() -#### Settings for rpath +# Settings for rpath if(NOT MSVC) - #add the option to enable RPATH + # Add the option to enable RPATH option(WB-TOOLBOX_ENABLE_RPATH "Enable RPATH installation" TRUE) mark_as_advanced(WB-TOOLBOX_ENABLE_RPATH) endif(NOT MSVC) @@ -38,22 +36,7 @@ add_install_rpath_support(BIN_DIRS ${CMAKE_INSTALL_PREFIX}/bin USE_LINK_PATH) find_package(Eigen3 REQUIRED) -find_package(wholeBodyInterface 0.2.5 REQUIRED) -find_package(yarpWholeBodyInterface 0.3.2 REQUIRED) -# Workaround section, aka, this stuff should be defined by imported libraries -find_package(iDynTree REQUIRED) - -#On new versions of Clang, MATLAB requires C++11. -#I enable it on all Clangs -if(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") - if(${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") - if (${CMAKE_GENERATOR} MATCHES "Xcode") - #this should set explictly the option in xcode. Unfortunately it does not work. - set(XCODE_ATTRIBUTE_CLANG_CXX_LANGUAGE_STANDARD "C++11") - endif(${CMAKE_GENERATOR} MATCHES "Xcode") - endif(${CMAKE_CXX_COMPILER_ID} MATCHES "Clang") -endif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin") +find_package(iDynTree 0.7.2 REQUIRED) include(Utilities) @@ -62,29 +45,32 @@ configure_block(BLOCK_NAME "Base" SOURCES src/base/toolbox.cpp src/base/Block.cpp src/base/BlockInformation.cpp - src/base/WBIBlock.cpp - src/base/Error.cpp - src/base/WBInterface.cpp + src/base/WBBlock.cpp + src/base/Log.cpp + src/base/ToolboxSingleton.cpp src/base/factory.cpp - src/base/WBIModelBlock.cpp - src/base/SimulinkBlockInformation.cpp #this is temp + src/base/SimulinkBlockInformation.cpp src/base/Signal.cpp + src/base/Configuration.cpp + src/base/RobotInterface.cpp HEADERS include/base/toolbox.h + include/base/debug.h include/base/Block.h include/base/BlockInformation.h - include/base/WBIBlock.h - include/base/Error.h - include/base/WBInterface.h - include/base/WBIModelBlock.h - include/base/SimulinkBlockInformation.h #this is temp + include/base/WBBlock.h + include/base/Log.h + include/base/ToolboxSingleton.h + include/base/SimulinkBlockInformation.h include/base/Signal.h + include/base/Configuration.h + include/base/RobotInterface.h ) -configure_block(BLOCK_NAME "Inverse Kinematics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/RemoteInverseKinematics.cpp - HEADERS include/RemoteInverseKinematics.h) +# configure_block(BLOCK_NAME "Inverse Kinematics" +# GROUP "Model" +# LIST_PREFIX WBT +# SOURCES src/RemoteInverseKinematics.cpp +# HEADERS include/RemoteInverseKinematics.h) option(WBT_USES_ICUB "Build models which need iCub library (e.g. Minimum Jerk Traj. Generator)" ON) if (WBT_USES_ICUB) @@ -97,17 +83,23 @@ if (WBT_USES_ICUB) SOURCES src/MinimumJerkTrajectoryGenerator.cpp HEADERS include/MinimumJerkTrajectoryGenerator.h) - if (${ICUB_USE_IPOPT}) - find_package(iDynTree REQUIRED) - add_definitions(-DWBT_USES_IPOPT) - configure_block(BLOCK_NAME "Inverse Kinematics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/InverseKinematics.cpp - HEADERS include/InverseKinematics.h) - - include_directories(SYSTEM ${iKin_INCLUDE_DIRS}) - endif() + # if (${ICUB_USE_IPOPT}) + # find_package(iDynTree REQUIRED) + # add_definitions(-DWBT_USES_IPOPT) + # configure_block(BLOCK_NAME "Inverse Kinematics" + # GROUP "Model" + # LIST_PREFIX WBT + # SOURCES src/InverseKinematics.cpp + # HEADERS include/InverseKinematics.h) + # + # include_directories(SYSTEM ${iKin_INCLUDE_DIRS}) + # endif() + + configure_block(BLOCK_NAME "Discrete Filter" + GROUP "Utilities" + LIST_PREFIX WBT + SOURCES src/DiscreteFilter.cpp + HEADERS include/DiscreteFilter.h) include_directories(SYSTEM ${ctrlLib_INCLUDE_DIRS}) endif() @@ -130,11 +122,11 @@ configure_block(BLOCK_NAME "Real Time Synchronizer" SOURCES src/RealTimeSynchronizer.cpp HEADERS include/RealTimeSynchronizer.h) -configure_block(BLOCK_NAME "YARP - WBI Converter" +configure_block(BLOCK_NAME "Model Partitioner" GROUP "Utilities" LIST_PREFIX WBT - SOURCES src/YARPWBIConverter.cpp - HEADERS include/YARPWBIConverter.h) + SOURCES src/ModelPartitioner.cpp + HEADERS include/ModelPartitioner.h) configure_block(BLOCK_NAME "Yarp Clock" GROUP "Utilities" @@ -142,13 +134,11 @@ configure_block(BLOCK_NAME "Yarp Clock" SOURCES src/YarpClock.cpp HEADERS include/YarpClock.h) -# TODO: this should be removed with a proper inclusion of the library -include_directories(SYSTEM "autogenerated/include/") configure_block(BLOCK_NAME "Simulator Synchronizer" GROUP "Utilities" LIST_PREFIX WBT - SOURCES src/SimulatorSynchronizer.cpp autogenerated/src/thrift/ClockServer.cpp - HEADERS include/SimulatorSynchronizer.h autogenerated/include/thrift/ClockServer.h) + SOURCES src/SimulatorSynchronizer.cpp + HEADERS include/SimulatorSynchronizer.h) configure_block(BLOCK_NAME "Mass Matrix" GROUP "Model" @@ -197,12 +187,12 @@ configure_block(BLOCK_NAME "Set Low-Level PIDs" LIST_PREFIX WBT SOURCES src/SetLowLevelPID.cpp HEADERS include/SetLowLevelPID.h) - -configure_block(BLOCK_NAME "Get Estimate" +# +configure_block(BLOCK_NAME "Get Measurement" GROUP "State" LIST_PREFIX WBT - SOURCES src/GetEstimate.cpp - HEADERS include/GetEstimate.h) + SOURCES src/GetMeasurement.cpp + HEADERS include/GetMeasurement.h) configure_block(BLOCK_NAME "Get Limits" GROUP "State" @@ -218,96 +208,61 @@ include_directories(include) include_directories(include/base) include_directories(SYSTEM ${Matlab_INCLUDE_DIRS} "${Matlab_ROOT_DIR}/simulink/include") -include_directories(SYSTEM ${YARP_INCLUDE_DIRS} ${yarpWholeBodyInterface_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) +include_directories(SYSTEM ${YARP_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) + +list(APPEND LINKED_LIBRARIES ${YARP_LIBRARIES}) +list(APPEND LINKED_LIBRARIES ${iDynTree_LIBRARIES}) -list(APPEND LINKED_LIBRARIES ${YARP_LIBRARIES} ${yarpWholeBodyInterface_LIBRARIES}) if (WBT_USES_ICUB) list(APPEND LINKED_LIBRARIES ctrlLib) if (${ICUB_USE_IPOPT}) - list(APPEND LINKED_LIBRARIES iKin ${iDynTree_LIBRARIES}) + list(APPEND LINKED_LIBRARIES iKin) endif() endif() -#Adding files used for the generation of the dynamic library -if (CMAKE_VERSION GREATER 3.0.0) - matlab_add_mex( - NAME WBToolbox - SRC ${ALL_HEADERS} ${ALL_SOURCES} - LINK_TO ${LINKED_LIBRARIES} - ) -else() - #Adding files used for the generation of the dynamic library - add_library(WBToolbox SHARED ${ALL_HEADERS} ${ALL_SOURCES}) - - set(SFUNCTION_SUFFIX ${Matlab_MEX_EXTENSION}) - #Removing default dynamic library extensions (.dll for Windows, .so for UNIX) and prefixes (lib) otherwise automatically prepended to the mex file. - set_target_properties(WBToolbox PROPERTIES SUFFIX .${SFUNCTION_SUFFIX}) - set_target_properties(WBToolbox PROPERTIES PREFIX "") - - # This seems to be the most reliable method to date to know installed Matlab arch - STRING(REGEX MATCH ..$ MATLAB_ARCH Matlab_MEX_EXTENSION 64) - - #In the following specific flags will be set to ensure compliance with the default settings of Matlab's mex compiler - if(WIN32) - if(NOT "${MATLAB_ARCH}" STREQUAL "64") - message("Flags have been passed to compiler for Matlab 32bits on a Windows computer") - elseif() - message("Flags have been passed to compiler for Matlab 64bits on a Windows computer") - endif() - if(MSVC) - #Setting Compiler Flags - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} /Z7 /nologo /W3 /WX- /O2 /Oy- /D_REENTRANT /D_CRT_SECURE_NO_DEPRECATE /D_SCL_SECURE_NO_DEPRECATE /D_SECURE_SCL=0 /DyWrite_EXPORTS /Gm- /EHs /Zp8 /GS /fp:precise /Zc:wchar_t /Zc:forScope /GR /Gd /TP /showIncludes") - #Setting Linker Flags - set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /INCREMENTAL:NO /NOLOGO /MANIFEST /debug /MAP.mexw64.map /MACHINE:X64 /EXPORT:mexFunction") - if(NOT "${MATLAB_ARCH}" STREQUAL "64") - #Changing some flags according to system/MATLAB version (32, 64 bits) - set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} /MAP.mexw32.map /MACHINE:X32") - endif() - elseif(MSVC) - message("You are not compiling using Microsoft Visual Studio, ERRORS might occur") - endif(MSVC) - elseif(WIN32) - message("No particular flags have been passed to compiler for Matlab 64bits on a UNIX computer") - endif(WIN32) - - # Linking Libraries - target_link_libraries(WBToolbox ${LINKED_LIBRARIES} ${Matlab_LIBRARIES}) -endif() +# Adding files used for the generation of the dynamic library +matlab_add_mex( + NAME WBToolbox + SRC ${ALL_HEADERS} ${ALL_SOURCES} + LINK_TO ${LINKED_LIBRARIES} +) -# Remote Inverse Kinematics requires C++11 -if (CMAKE_VERSION VERSION_LESS "3.1") - if (CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR - CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - target_compile_options(WBToolbox PUBLIC "-std=c++11") - endif() -else() - set(CXX_STANDARD_REQUIRED ON) - set_property(TARGET WBToolbox PROPERTY CXX_STANDARD 11) -endif() +# Link with MxAnyType library +# TODO: this will become an external project sooner or later +target_link_libraries(WBToolbox MxAnyType) -install(TARGETS WBToolbox DESTINATION ${CMAKE_INSTALL_PREFIX}/mex) +# Link with ClockServer library +add_subdirectory(autogenerated/) +target_link_libraries(WBToolbox ClockServer) -## if you export the mdl / slx library, remeber to call this command in matlab before saving it -## set_param(gcs, 'EnableLBRepository','on'); +install(TARGETS WBToolbox DESTINATION ${CMAKE_INSTALL_PREFIX}/mex) # The following line is to properly configure the installation script of the toolbox -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/.startup_wbitoolbox.m.in ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m) -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/.setupForMatlabDebug.m.in ${CMAKE_BINARY_DIR}/setupForMatlabDebug.m) +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/matlab/startup_wbitoolbox.m.in + ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m) # Custom script to generate the library to be committed on the repository # This target is excluded from the normal build and must be called explicitly by the # developer who modifies the library -add_custom_target(export_libraries ${Matlab_MAIN_PROGRAM} -nosplash -r export_library WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) +add_custom_target(export_libraries ${Matlab_MAIN_PROGRAM} -nosplash -nodesktop -r export_library WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab) set_target_properties(export_libraries PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD 1) # Install configuration files install(FILES ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) -install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/slblocks.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/slblocks.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) + +# Install the package folder +install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab/+WBToolbox + DESTINATION ${WB-TOOLBOX_SHARE_DIR}) + #if MAJOR >= 2014 && MINOR >= b # Note: We had issues with Matlab 2014b and .mdl models. # But this issue seems to have been disappeared in 2015b. We have to check if we need to enable this if again -if (${Matlab_VERSION_STRING} GREATER "8.3") - install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.slx DESTINATION ${WB-TOOLBOX_SHARE_DIR}) -else() - install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.mdl DESTINATION ${WB-TOOLBOX_SHARE_DIR}) -endif() +# TODO: check if the mdl support is still required +# if (${Matlab_VERSION_STRING} GREATER "8.3") + # install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.slx DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +# else() + # install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.mdl DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +# endif() +install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/library/exported/WBToolboxLibrary.slx + DESTINATION ${WB-TOOLBOX_SHARE_DIR}) diff --git a/toolbox/WBToolboxLibrary.mdl b/toolbox/WBToolboxLibrary.mdl index a0e924d52..4df715224 100644 --- a/toolbox/WBToolboxLibrary.mdl +++ b/toolbox/WBToolboxLibrary.mdl @@ -8,15 +8,15 @@ Library { DisableAllScopes off FPTRunName "Run 1" MaxMDLFileLineLength 120 - Created "Thu Feb 06 02:21:39 2014" + Created "Thu Feb 06 01:21:39 2014" Creator "jorhabib" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" - LastModifiedBy "makaveli" + LastModifiedBy "icub" ModifiedDateFormat "%" - LastModifiedDate "Wed Aug 23 12:36:18 2017" - RTWModifiedTimeStamp 425392578 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Aug 11 12:52:09 2017" + RTWModifiedTimeStamp 424356729 + ModelVersionFormat "1.%" ConfigurationManager "none" SampleTimeColors off SampleTimeAnnotations off @@ -755,7 +755,7 @@ Library { } System { Name "WBToolboxLibrary" - Location [747, 53, 2027, 774] + Location [123, 132, 1403, 853] Open off ModelBrowserVisibility off ModelBrowserWidth 200 @@ -769,7 +769,7 @@ Library { ShowPageBoundaries off ZoomFactor "273" ReportName "simulink-default.rpt" - SIDHighWatermark "1773" + SIDHighWatermark "1772" Block { BlockType SubSystem Name "Utilities" @@ -789,7 +789,7 @@ Library { MaskIconUnits "autoscale" System { Name "Utilities" - Location [551, 44, 1831, 765] + Location [447, 302, 1727, 1023] Open off ModelBrowserVisibility off ModelBrowserWidth 200 @@ -801,7 +801,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "166" + ZoomFactor "175" Block { BlockType SubSystem Name "DampPinv" @@ -896,7 +896,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1773" + SIDHighWatermark "1772" Block { BlockType Inport Name "mat" @@ -917,20 +917,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "107::1618" + SID "107::1627" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 107 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "107::1617" + SID "107::1626" Tag "Stateflow S-Function WBToolboxLibrary 6" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 97 + ZOrder 106 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -945,9 +945,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "107::1619" + SID "107::1628" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 108 } Block { BlockType Outport @@ -1032,12 +1032,59 @@ Library { Opaque off MaskHideContents off } + Block { + BlockType S-Function + Name "DiscreteFilter" + SID "1772" + Ports [1, 1] + Position [400, 245, 460, 275] + ZOrder 82 + BackgroundColor "yellow" + FunctionName "WBToolbox" + Parameters "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0" + SFunctionDeploymentMode off + EnableBusSupport off + MaskType "Discrete Filter" + MaskDescription "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." + MaskPromptString "Type of the filter|Numerator Coefficients*|Denominator Coefficients*|Cut Frequency (Hz)|Samplin" + "g time (s)|Order|Define initial conditions|Output y0|Input u0" + MaskStyleString "popup(Generic|FirstOrderLowPassFilter|MedianFilter),edit,edit,edit,edit,edit,checkbox,edit,edit" + MaskVariables "filterType=@1;numCoeffs=@2;denCoeffs=@3;Fc=@4;Ts=@5;orderMedianFilter=@6;initStatus=@7;y0=@8;u0=@" + "9;" + MaskTunableValueString "on,on,on,on,on,on,on,on,on" + MaskCallbackString "% From: https://it.mathworks.com/help/simulink/slref/simulink.mask-class.html\nfilterType = g" + "et_param(gcb, 'filterType');\ninitStatus = get_param(gcb, 'initStatus');\np = Simulink.Mask.get(gcbh);\nhowToCoeffs" + " = p.getDialogControl('howToCoeffs');\n\n%set_param(gcb, 'initStatus','off');\nif (strcmp(initStatus,'on'))\n vi" + "s_init = 'on';\nelse\n vis_init = 'off';\nend\n\nif(strcmp(filterType, 'Generic'))\n set_param(gcb, 'MaskVisi" + "bilities',{'on','on','on','off','off','off','on',vis_init,vis_init});\n howToCoeffs.Visible = 'on';\nelseif(strc" + "mp(filterType, 'FirstOrderLowPassFilter'))\n set_param(gcb, 'MaskVisibilities',{'on','off','off','on','on','off'" + ",'on',vis_init,'off'});\n howToCoeffs.Visible = 'off';\nelseif(strcmp(filterType, 'MedianFilter'))\n set_para" + "m(gcb, 'MaskVisibilities',{'on','off','off','off','off','on','on',vis_init,'off'});\n howToCoeffs.Visible = 'off" + "';\nend\n\nclear filterType initStatus p howToCoeffs vis_init;||||||initStatus = get_param(gcb, 'initStatus');\nv" + "isibilities = get_param(gcb, 'MaskVisibilities');\nfilterType = get_param(gcb, 'filterType');\n\nif (strcmp(initS" + "tatus,'off'))\n visibilities{8} = 'off';\n visibilities{9} = 'off';\nelseif (strcmp(initStatus,'on'))\n vi" + "sibilities{8} = 'on';\n if (strcmp(filterType,'Generic'))\n visibilities{9} = 'on';\n end\nend\n\nset_" + "param(gcb, 'MaskVisibilities', visibilities);\n\nclear initStatus visibilities filterType;||" + MaskEnableString "on,on,on,on,on,on,on,on,on" + MaskVisibilityString "on,on,on,off,off,off,on,off,off" + MaskToolTipString "on,on,on,on,on,on,on,on,on" + MaskInitialization "filterType = get_param(gcb, 'filterType');\nnumCoeffs = mat2str(numCoeffs);\ndenCoeffs = ma" + "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n y0 = mat2str(y" + "0);\n u0 = mat2str(u0);\nelse\n y0 = 'none';\n u0 = 'none';\nend\n\nclear initStatus" + MaskDisplay "disp('Filter')" + MaskIconFrame on + MaskIconOpaque on + MaskIconRotate "none" + MaskPortRotate "default" + MaskIconUnits "autoscale" + MaskValueString "Generic|[0]|[0]|0|0|0|off|[0]|[0]" + } Block { BlockType S-Function Name "DoFs converter" SID "1771" Ports [1, 5] - Position [240, 241, 385, 309] + Position [160, 226, 305, 294] ZOrder 81 InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" @@ -1079,7 +1126,7 @@ Library { Name "Minimum Jerk Trajectory Generator" SID "1747" Ports [1, 3] - Position [450, 73, 605, 127] + Position [335, 73, 490, 127] ZOrder 78 FunctionName "WBToolbox" Parameters "'MinimumJerkTrajectoryGenerator',sampleTime, settlingTime, firstDerivatives, secondDerivatives, expl" @@ -1106,7 +1153,7 @@ Library { "else\n visibility{2} = 'on';\n visibility{4} = 'off';\nend\nset_param(gcb, 'MaskVisibilities',visibility);\nc" "lear externalSettlingTime||||||" MaskEnableString "on,on,on,on,on,on,on" - MaskVisibilityString "on,on,on,on,on,on,off" + MaskVisibilityString "on,on,on,off,on,on,off" MaskToolTipString "on,on,on,on,on,on,on" MaskDisplay "firstDer = get_param(gcb, 'firstDerivatives');\nsecondDer = get_param(gcb, 'secondDerivatives');\ni" "nitialValues = get_param(gcb, 'explicitInitialValue');\nexternalSettlingTimeParam = get_param(gcb, 'externalSettlin" @@ -1283,7 +1330,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1773" + SIDHighWatermark "1772" Block { BlockType Inport Name "mat" @@ -1304,20 +1351,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "112::1609" + SID "112::1618" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 107 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "112::1608" + SID "112::1617" Tag "Stateflow S-Function WBToolboxLibrary 7" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 97 + ZOrder 106 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -1332,9 +1379,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "112::1610" + SID "112::1619" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 108 } Block { BlockType Outport @@ -1419,29 +1466,6 @@ Library { Opaque off MaskHideContents off } - Block { - BlockType S-Function - Name "Yarp Clock" - SID "1773" - Ports [0, 1] - Position [335, 76, 405, 124] - ZOrder 85 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpClock'" - SFunctionDeploymentMode off - EnableBusSupport off - MaskType "YARP Clock" - MaskDescription "This block outputs the current YARP Time\nIn a nutshell, this block outputs the equivalent of th" - "e C++ function call\nyarp::os::Time::now()" - MaskSelfModifiable on - MaskDisplay "disp('YARP Time')\n" - MaskIconFrame on - MaskIconOpaque on - MaskIconRotate "none" - MaskPortRotate "default" - MaskIconUnits "autoscale" - } Block { BlockType S-Function Name "Yarp Read" @@ -1764,7 +1788,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1773" + SIDHighWatermark "1772" Block { BlockType Inport Name "s" @@ -1785,20 +1809,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "1300::1609" + SID "1300::1618" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 86 + ZOrder 95 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "1300::1608" + SID "1300::1617" Tag "Stateflow S-Function WBToolboxLibrary 1" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 85 + ZOrder 94 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -1813,9 +1837,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "1300::1610" + SID "1300::1619" Position [460, 241, 480, 259] - ZOrder 87 + ZOrder 96 } Block { BlockType Outport @@ -3912,7 +3936,7 @@ Library { Annotation { SID "1213" Name "WHOLE BODY TOOLBOX" - Position [241, 157] + Position [244, 157] ForegroundColor "white" BackgroundColor "black" FontName "Sans Serif" @@ -3923,7 +3947,7 @@ Library { Name "Copyright 2013-2016 iCub Facility Department, Istituto Italiano di Tecnologia\nThis software was s" "upported by the FP7 EU Project CoDyCO (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b))\nhttp://www.c" "odyco.eu" - Position [240, 192] + Position [241, 194] ForegroundColor "white" BackgroundColor "black" FontSize 5 diff --git a/toolbox/WBToolboxLibrary.slx b/toolbox/WBToolboxLibrary.slx index e19fa448e..9264c236b 100644 Binary files a/toolbox/WBToolboxLibrary.slx and b/toolbox/WBToolboxLibrary.slx differ diff --git a/toolbox/WBToolboxLibrary_repository.mdl b/toolbox/WBToolboxLibrary_repository.mdl index 357430b63..b5034389e 100644 --- a/toolbox/WBToolboxLibrary_repository.mdl +++ b/toolbox/WBToolboxLibrary_repository.mdl @@ -6,14 +6,14 @@ Library { LogicAnalyzerPlugin "on" LogicAnalyzerSignalOrdering "" DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 3 0 0 0 0 0" + SuppressorTable "22 serialization::archive 11 0 3 0 0 0 11 0" LibraryType "BlockLibrary" ScopeRefreshTime 0.035000 OverrideScopeRefreshTime on DisableAllScopes off FPTRunName "Run 1" MaxMDLFileLineLength 120 - LastSavedArchitecture "maci64" + LastSavedArchitecture "glnxa64" Object { $PropName "BdWindowsInfo" $ObjectID 1 @@ -23,7 +23,7 @@ Library { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [747.0, 53.0, 1280.0, 721.0] + Location [447.0, 302.0, 1280.0, 721.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -45,11 +45,11 @@ Library { $ObjectID 5 $ClassName "Simulink.EditorInfo" IsActive [1] - ViewObjType "SimulinkTopLevel" - LoadSaveID "0" - Extents [1246.0, 559.0] - ZoomFactor [2.7309417040358746] - Offset [10.873563218390814, 6.1543513957307141] + ViewObjType "SimulinkSubsys" + LoadSaveID "192" + Extents [1226.0, 537.0] + ZoomFactor [1.75] + Offset [10.634251290877842, 6.0] } Object { $PropName "DockComponentsInfo" @@ -67,24 +67,24 @@ Library { } WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABvAP///wAAAAEAAAAAAAAAAPwCA" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAABeAP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAKgD///8AAAUAAAACZgAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAKAD///8AAATsAAACUwAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACE/////wAAAAAAAAAA/////wEAAAD2/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFL/////wAAAAAAAAAA/" + "////wEAAACA/////wAAAAAAAAAA/////wEAAADo/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAE7/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" } } - Created "Thu Feb 06 02:21:39 2014" + Created "Thu Feb 06 01:21:39 2014" Creator "jorhabib" UpdateHistory "UpdateHistoryNever" ModifiedByFormat "%" - LastModifiedBy "makaveli" + LastModifiedBy "icub" ModifiedDateFormat "%" - LastModifiedDate "Wed Aug 23 12:33:30 2017" - RTWModifiedTimeStamp 425392371 - ModelVersionFormat "1.%" + LastModifiedDate "Fri Aug 11 12:50:36 2017" + RTWModifiedTimeStamp 424356590 + ModelVersionFormat "1.%" ConfigurationManager "none" SampleTimeColors off SampleTimeAnnotations off @@ -1031,8 +1031,8 @@ Library { } System { Name "WBToolboxLibrary_repository" - Location [747, 53, 2027, 774] - Open on + Location [123, 132, 1403, 853] + Open off ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "black" @@ -1045,7 +1045,7 @@ Library { ShowPageBoundaries off ZoomFactor "273" ReportName "simulink-default.rpt" - SIDHighWatermark "1773" + SIDHighWatermark "1772" Block { BlockType SubSystem Name "Utilities" @@ -1065,8 +1065,8 @@ Library { } System { Name "Utilities" - Location [551, 44, 1831, 765] - Open off + Location [447, 302, 1727, 1023] + Open on ModelBrowserVisibility off ModelBrowserWidth 200 ScreenColor "white" @@ -1077,7 +1077,7 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "166" + ZoomFactor "175" Block { BlockType SubSystem Name "DampPinv" @@ -1163,7 +1163,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1619" + SIDHighWatermark "1628" Block { BlockType Inport Name "mat" @@ -1184,20 +1184,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "107::1618" + SID "107::1627" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 107 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "107::1617" + SID "107::1626" Tag "Stateflow S-Function WBToolboxLibrary_repository 6" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 97 + ZOrder 106 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -1213,9 +1213,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "107::1619" + SID "107::1628" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 108 } Block { BlockType Outport @@ -1226,7 +1226,7 @@ Library { IconDisplay "Port number" } Line { - ZOrder 61 + ZOrder 76 SrcBlock "mat" SrcPort 1 Points [120, 0] @@ -1234,7 +1234,7 @@ Library { DstPort 1 } Line { - ZOrder 62 + ZOrder 77 SrcBlock "sigma" SrcPort 1 DstBlock " SFunction " @@ -1242,7 +1242,7 @@ Library { } Line { Name "DPinv" - ZOrder 63 + ZOrder 78 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1250,14 +1250,14 @@ Library { DstPort 1 } Line { - ZOrder 64 + ZOrder 79 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 65 + ZOrder 80 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1296,12 +1296,223 @@ Library { } } } + Block { + BlockType S-Function + Name "DiscreteFilter" + SID "1772" + Ports [1, 1] + Position [400, 245, 460, 275] + ZOrder 82 + BackgroundColor "yellow" + FunctionName "WBToolbox" + Parameters "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 23 + $ClassName "Simulink.Mask" + Type "Discrete Filter" + Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." + Initialization "filterType = get_param(gcb, 'filterType');\nnumCoeffs = mat2str(numCoeffs);\ndenCoeffs = ma" + "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n y0 = mat2str" + "(y0);\n u0 = mat2str(u0);\nelse\n y0 = 'none';\n u0 = 'none';\nend\n\nclear initStatus" + Display "disp('Filter')" + Array { + Type "Simulink.MaskParameter" + Dimension 9 + Object { + $ObjectID 24 + Type "popup" + Array { + Type "Cell" + Dimension 3 + Cell "Generic" + Cell "FirstOrderLowPassFilter" + Cell "MedianFilter" + PropName "TypeOptions" + } + Name "filterType" + Prompt "Type of the filter" + Value "Generic" + Callback "% From: https://it.mathworks.com/help/simulink/slref/simulink.mask-class.html\nfilterType = get_param(gcb" + ", 'filterType');\ninitStatus = get_param(gcb, 'initStatus');\np = Simulink.Mask.get(gcbh);\nhowToCoeffs = p.getDialo" + "gControl('howToCoeffs');\n\n%set_param(gcb, 'initStatus','off');\nif (strcmp(initStatus,'on'))\n vis_init = 'on';" + "\nelse\n vis_init = 'off';\nend\n\nif(strcmp(filterType, 'Generic'))\n set_param(gcb, 'MaskVisibilities',{'on'" + ",'on','on','off','off','off','on',vis_init,vis_init});\n howToCoeffs.Visible = 'on';\nelseif(strcmp(filterType, '" + "FirstOrderLowPassFilter'))\n set_param(gcb, 'MaskVisibilities',{'on','off','off','on','on','off','on',vis_init,'o" + "ff'});\n howToCoeffs.Visible = 'off';\nelseif(strcmp(filterType, 'MedianFilter'))\n set_param(gcb, 'MaskVisibi" + "lities',{'on','off','off','off','off','on','on',vis_init,'off'});\n howToCoeffs.Visible = 'off';\nend\n\nclear fi" + "lterType initStatus p howToCoeffs vis_init;" + } + Object { + $ObjectID 25 + Type "edit" + Name "numCoeffs" + Prompt "Numerator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 26 + Type "edit" + Name "denCoeffs" + Prompt "Denominator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 27 + Type "edit" + Name "Fc" + Prompt "Cut Frequency (Hz)" + Value "0" + Visible "off" + } + Object { + $ObjectID 28 + Type "edit" + Name "Ts" + Prompt "Sampling time (s)" + Value "0" + Visible "off" + } + Object { + $ObjectID 29 + Type "edit" + Name "orderMedianFilter" + Prompt "Order" + Value "0" + Visible "off" + } + Object { + $ObjectID 30 + Type "checkbox" + Name "initStatus" + Prompt "Define initial conditions" + Value "off" + Callback "initStatus = get_param(gcb, 'initStatus');\nvisibilities = get_param(gcb, 'MaskVisibilities');\nfilterT" + "ype = get_param(gcb, 'filterType');\n\nif (strcmp(initStatus,'off'))\n visibilities{8} = 'off';\n visibiliti" + "es{9} = 'off';\nelseif (strcmp(initStatus,'on'))\n visibilities{8} = 'on';\n if (strcmp(filterType,'Generic'))" + "\n visibilities{9} = 'on';\n end\nend\n\nset_param(gcb, 'MaskVisibilities', visibilities);\n\nclear initSt" + "atus visibilities filterType;" + } + Object { + $ObjectID 31 + Type "edit" + Name "y0" + Prompt "Output y0" + Value "[0]" + Visible "off" + } + Object { + $ObjectID 32 + Type "edit" + Name "u0" + Prompt "Input u0" + Value "[0]" + Visible "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 33 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 34 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 35 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 8 + Object { + $ObjectID 36 + $ClassName "Simulink.dialog.parameter.Popup" + Name "filterType" + } + Object { + $ObjectID 37 + $ClassName "Simulink.dialog.parameter.Edit" + Name "numCoeffs" + } + Object { + $ObjectID 38 + $ClassName "Simulink.dialog.parameter.Edit" + Name "denCoeffs" + } + Object { + $ObjectID 39 + $ClassName "Simulink.dialog.Text" + Prompt "* The coefficients are ordered in increasing power of z^-1" + Name "howToCoeffs" + } + Object { + $ObjectID 40 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Fc" + } + Object { + $ObjectID 41 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Ts" + } + Object { + $ObjectID 42 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "orderMedianFilter" + } + Object { + $ObjectID 43 + $ClassName "Simulink.dialog.Group" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 44 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "initStatus" + } + Object { + $ObjectID 45 + $ClassName "Simulink.dialog.parameter.Edit" + Name "y0" + } + Object { + $ObjectID 46 + $ClassName "Simulink.dialog.parameter.Edit" + Name "u0" + } + PropName "DialogControls" + } + Name "Container3" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } Block { BlockType S-Function Name "DoFs converter" SID "1771" Ports [1, 5] - Position [240, 241, 385, 309] + Position [160, 226, 305, 294] ZOrder 81 InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var')\n W" "BT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWholeBodyInterf" @@ -1313,7 +1524,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 23 + $ObjectID 47 $ClassName "Simulink.Mask" Type "DoFs converter" Description "This block converts from a DoFs serialization to a YARP serialization or viceversa depending on" @@ -1332,7 +1543,7 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 24 + $ObjectID 48 Type "popup" Array { Type "Cell" @@ -1346,28 +1557,28 @@ Library { Value "From YARP to WBI" } Object { - $ObjectID 25 + $ObjectID 49 Type "edit" Name "robotName" Prompt "Robot Port Name" Value "WBT_robotName" } Object { - $ObjectID 26 + $ObjectID 50 Type "edit" Name "localName" Prompt "Model Name" Value "WBT_modelName" } Object { - $ObjectID 27 + $ObjectID 51 Type "edit" Name "wbiFile" Prompt "WBI filename" Value "WBT_wbiFilename" } Object { - $ObjectID 28 + $ObjectID 52 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -1379,11 +1590,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 29 + $ObjectID 53 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 30 + $ObjectID 54 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1391,46 +1602,46 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 31 + $ObjectID 55 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 32 + $ObjectID 56 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 33 + $ObjectID 57 Prompt "Block Parameters" Object { $PropName "DialogControls" - $ObjectID 34 + $ObjectID 58 $ClassName "Simulink.dialog.parameter.Popup" Name "yarp2WBIOption" } Name "Container4" } Object { - $ObjectID 35 + $ObjectID 59 Prompt "WBI Parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 36 + $ObjectID 60 Name "robotName" } Object { - $ObjectID 37 + $ObjectID 61 Name "localName" } Object { - $ObjectID 38 + $ObjectID 62 Name "wbiFile" } Object { - $ObjectID 39 + $ObjectID 63 Name "wbiList" } PropName "DialogControls" @@ -1452,7 +1663,7 @@ Library { Name "Minimum Jerk Trajectory Generator" SID "1747" Ports [1, 3] - Position [450, 73, 605, 127] + Position [335, 73, 490, 127] ZOrder 78 FunctionName "WBToolbox" Parameters "'MinimumJerkTrajectoryGenerator',sampleTime, settlingTime, firstDerivatives, secondDerivatives, expl" @@ -1462,7 +1673,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 40 + $ObjectID 64 $ClassName "Simulink.Mask" Type "Minimum Jerk Trajectory Generator" Description "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk " @@ -1485,7 +1696,7 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 41 + $ObjectID 65 Type "checkbox" Name "externalSettlingTime" Prompt "External Settling Time" @@ -1496,42 +1707,43 @@ Library { "SettlingTime" } Object { - $ObjectID 42 + $ObjectID 66 Type "edit" Name "settlingTime" Prompt "Settling Time" Value "3" } Object { - $ObjectID 43 + $ObjectID 67 Type "edit" Name "sampleTime" Prompt "Sample Time" Value "0.01" } Object { - $ObjectID 44 + $ObjectID 68 Type "checkbox" Name "resetOnSettlingTime" Prompt "Reset on Settling Time Changes" Value "off" + Visible "off" } Object { - $ObjectID 45 + $ObjectID 69 Type "checkbox" Name "firstDerivatives" Prompt "Output First Derivative" Value "on" } Object { - $ObjectID 46 + $ObjectID 70 Type "checkbox" Name "secondDerivatives" Prompt "Output Second Derivative" Value "on" } Object { - $ObjectID 47 + $ObjectID 71 Type "checkbox" Name "explicitInitialValue" Prompt "Explicit Initial Value" @@ -1544,11 +1756,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 48 + $ObjectID 72 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 49 + $ObjectID 73 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1556,38 +1768,38 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 50 + $ObjectID 74 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 51 + $ObjectID 75 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 52 + $ObjectID 76 Prompt "Trajectory Parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 4 Object { - $ObjectID 53 + $ObjectID 77 $ClassName "Simulink.dialog.parameter.CheckBox" Name "externalSettlingTime" } Object { - $ObjectID 54 + $ObjectID 78 $ClassName "Simulink.dialog.parameter.Edit" Name "settlingTime" } Object { - $ObjectID 55 + $ObjectID 79 $ClassName "Simulink.dialog.parameter.Edit" Name "sampleTime" } Object { - $ObjectID 56 + $ObjectID 80 $ClassName "Simulink.dialog.parameter.CheckBox" Name "resetOnSettlingTime" } @@ -1596,21 +1808,21 @@ Library { Name "Tab1" } Object { - $ObjectID 57 + $ObjectID 81 Prompt "Input/Output" Array { Type "Simulink.dialog.parameter.CheckBox" Dimension 3 Object { - $ObjectID 58 + $ObjectID 82 Name "firstDerivatives" } Object { - $ObjectID 59 + $ObjectID 83 Name "secondDerivatives" } Object { - $ObjectID 60 + $ObjectID 84 Name "explicitInitialValue" } PropName "DialogControls" @@ -1644,7 +1856,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 61 + $ObjectID 85 $ClassName "Simulink.Mask" Type "Real Time Synchronizer" Description "This block slows down the simulation trying to match the period specified \nas parameter (in se" @@ -1652,7 +1864,7 @@ Library { Display "disp('Real Time Synchronizer')" Object { $PropName "Parameters" - $ObjectID 62 + $ObjectID 86 $ClassName "Simulink.MaskParameter" Type "edit" Name "period" @@ -1677,7 +1889,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 63 + $ObjectID 87 $ClassName "Simulink.Mask" Type "Simulator Synchronizer" Description "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported" @@ -1687,21 +1899,21 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 64 + $ObjectID 88 Type "edit" Name "period" Prompt "Controller Period (in seconds)" Value "0.01" } Object { - $ObjectID 65 + $ObjectID 89 Type "edit" Name "serverPortName" Prompt "Server Port Name" Value "'/clock/rpc'" } Object { - $ObjectID 66 + $ObjectID 90 Type "edit" Name "clientPortName" Prompt "Client Port Name" @@ -1723,11 +1935,11 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 67 + $ObjectID 91 $ClassName "Simulink.Mask" Object { $PropName "Parameters" - $ObjectID 68 + $ObjectID 92 $ClassName "Simulink.MaskParameter" Type "edit" Name "tol" @@ -1796,7 +2008,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1610" + SIDHighWatermark "1619" Block { BlockType Inport Name "mat" @@ -1817,20 +2029,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "112::1609" + SID "112::1618" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 98 + ZOrder 107 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "112::1608" + SID "112::1617" Tag "Stateflow S-Function WBToolboxLibrary_repository 7" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 97 + ZOrder 106 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -1846,9 +2058,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "112::1610" + SID "112::1619" Position [460, 241, 480, 259] - ZOrder 99 + ZOrder 108 } Block { BlockType Outport @@ -1859,7 +2071,7 @@ Library { IconDisplay "Port number" } Line { - ZOrder 61 + ZOrder 76 SrcBlock "mat" SrcPort 1 Points [120, 0] @@ -1867,7 +2079,7 @@ Library { DstPort 1 } Line { - ZOrder 62 + ZOrder 77 SrcBlock "tol" SrcPort 1 DstBlock " SFunction " @@ -1875,7 +2087,7 @@ Library { } Line { Name "TPinv" - ZOrder 63 + ZOrder 78 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -1883,14 +2095,14 @@ Library { DstPort 1 } Line { - ZOrder 64 + ZOrder 79 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 65 + ZOrder 80 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -1929,30 +2141,6 @@ Library { } } } - Block { - BlockType S-Function - Name "Yarp Clock" - SID "1773" - Ports [0, 1] - Position [335, 76, 405, 124] - ZOrder 85 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpClock'" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 69 - $ClassName "Simulink.Mask" - Type "YARP Clock" - Description "This block outputs the current YARP Time\nIn a nutshell, this block outputs the equivalent of t" - "he C++ function call\nyarp::os::Time::now()" - SelfModifiable "on" - Display "disp('YARP Time')\n" - } - } Block { BlockType S-Function Name "Yarp Read" @@ -1968,7 +2156,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 70 + $ObjectID 93 $ClassName "Simulink.Mask" Type "YARP Read" Description "This block behaves as the command 'yarp read'. \nBy default it has the option 'Autoconnect' act" @@ -1986,35 +2174,35 @@ Library { Type "Simulink.MaskParameter" Dimension 6 Object { - $ObjectID 71 + $ObjectID 94 Type "edit" Name "portName" Prompt "Source Port Name" Value "'/portname'" } Object { - $ObjectID 72 + $ObjectID 95 Type "edit" Name "signalSize" Prompt "Port Size" Value "1" } Object { - $ObjectID 73 + $ObjectID 96 Type "checkbox" Name "blocking" Prompt "Blocking Read" Value "off" } Object { - $ObjectID 74 + $ObjectID 97 Type "checkbox" Name "timestamp" Prompt "Read Timestamp" Value "on" } Object { - $ObjectID 75 + $ObjectID 98 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2026,7 +2214,7 @@ Library { "t_string" } Object { - $ObjectID 76 + $ObjectID 99 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2051,7 +2239,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 77 + $ObjectID 100 $ClassName "Simulink.Mask" Type "YARP Write" Description "This block behaves as the command 'yarp write'. \n\nBy default this block opens a port names as" @@ -2064,14 +2252,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 78 + $ObjectID 101 Type "edit" Name "portName" Prompt "Opened Port Name" Value "'/portname'" } Object { - $ObjectID 79 + $ObjectID 102 Type "checkbox" Name "autoconnect" Prompt "Autoconnect" @@ -2082,7 +2270,7 @@ Library { "';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string" } Object { - $ObjectID 80 + $ObjectID 103 Type "checkbox" Name "errorOnConnection" Prompt "Error on missing connection" @@ -2105,7 +2293,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 81 + $ObjectID 104 $ClassName "Simulink.Mask" Type "Errors" Description "Computes two kinds of errors. The first is just the difference between x\nand y while the secon" @@ -2248,7 +2436,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 82 + $ObjectID 105 $ClassName "Simulink.Mask" Type "Holder" Description "This block holds the first input value during the simulation." @@ -2328,7 +2516,7 @@ Library { TiledPageScale 1 ShowPageBoundaries off ZoomFactor "100" - SIDHighWatermark "1610" + SIDHighWatermark "1619" Block { BlockType Inport Name "s" @@ -2349,20 +2537,20 @@ Library { Block { BlockType Demux Name " Demux " - SID "1300::1609" + SID "1300::1618" Ports [1, 1] Position [270, 230, 320, 270] - ZOrder 86 + ZOrder 95 Outputs "1" } Block { BlockType S-Function Name " SFunction " - SID "1300::1608" + SID "1300::1617" Tag "Stateflow S-Function WBToolboxLibrary_repository 1" Ports [2, 2] Position [180, 100, 230, 160] - ZOrder 85 + ZOrder 94 FunctionName "sf_sfun" PortCounts "[2 2]" SFunctionDeploymentMode off @@ -2378,9 +2566,9 @@ Library { Block { BlockType Terminator Name " Terminator " - SID "1300::1610" + SID "1300::1619" Position [460, 241, 480, 259] - ZOrder 87 + ZOrder 96 } Block { BlockType Outport @@ -2391,7 +2579,7 @@ Library { IconDisplay "Port number" } Line { - ZOrder 61 + ZOrder 76 SrcBlock "s" SrcPort 1 Points [120, 0] @@ -2399,7 +2587,7 @@ Library { DstPort 1 } Line { - ZOrder 62 + ZOrder 77 SrcBlock "unused" SrcPort 1 DstBlock " SFunction " @@ -2407,7 +2595,7 @@ Library { } Line { Name "s0" - ZOrder 63 + ZOrder 78 Labels [0, 0] SrcBlock " SFunction " SrcPort 2 @@ -2415,14 +2603,14 @@ Library { DstPort 1 } Line { - ZOrder 64 + ZOrder 79 SrcBlock " Demux " SrcPort 1 DstBlock " Terminator " DstPort 1 } Line { - ZOrder 65 + ZOrder 80 SrcBlock " SFunction " SrcPort 1 DstBlock " Demux " @@ -2483,7 +2671,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 83 + $ObjectID 106 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyActuators.png'),'center')" } @@ -2517,7 +2705,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 84 + $ObjectID 107 $ClassName "Simulink.Mask" Type "Set Low Level PIDs" Description "This block sets the low level PID values for the robot actuators.\n\nIf only one gain file is s" @@ -2536,7 +2724,7 @@ Library { Type "Simulink.MaskParameter" Dimension 6 Object { - $ObjectID 85 + $ObjectID 108 Type "edit" Name "pidParameters" Prompt "PID parameter" @@ -2544,7 +2732,7 @@ Library { Tunable "off" } Object { - $ObjectID 86 + $ObjectID 109 Type "popup" Array { Type "Cell" @@ -2564,7 +2752,7 @@ Library { "kStr, 'Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear maskStr" } Object { - $ObjectID 87 + $ObjectID 110 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -2572,7 +2760,7 @@ Library { Tunable "off" } Object { - $ObjectID 88 + $ObjectID 111 Type "edit" Name "localName" Prompt "Model Name" @@ -2580,7 +2768,7 @@ Library { Tunable "off" } Object { - $ObjectID 89 + $ObjectID 112 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -2588,7 +2776,7 @@ Library { Tunable "off" } Object { - $ObjectID 90 + $ObjectID 113 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -2601,11 +2789,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 91 + $ObjectID 114 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 92 + $ObjectID 115 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2613,29 +2801,29 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 93 + $ObjectID 116 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 94 + $ObjectID 117 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 95 + $ObjectID 118 Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 2 Object { - $ObjectID 96 + $ObjectID 119 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "pidParameters" } Object { - $ObjectID 97 + $ObjectID 120 $ClassName "Simulink.dialog.parameter.Popup" Name "pidType" } @@ -2644,25 +2832,25 @@ Library { Name "Container8" } Object { - $ObjectID 98 + $ObjectID 121 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 99 + $ObjectID 122 Name "robotName" } Object { - $ObjectID 100 + $ObjectID 123 Name "localName" } Object { - $ObjectID 101 + $ObjectID 124 Name "wbiFile" } Object { - $ObjectID 102 + $ObjectID 125 Name "wbiList" } PropName "DialogControls" @@ -2723,7 +2911,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 103 + $ObjectID 126 $ClassName "Simulink.Mask" Type "Set References" Description "This block sets the references for the robot actuators.\nThe type of control mode is specified " @@ -2740,7 +2928,7 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 104 + $ObjectID 127 Type "popup" Array { Type "Cell" @@ -2764,7 +2952,7 @@ Library { "'Open Loop')\n set_param(gcb,'BackgroundColor', '[0.98, 0.98, 0.98, 1.0]');\nend\nclear maskStr" } Object { - $ObjectID 105 + $ObjectID 128 Type "popup" Array { Type "Cell" @@ -2781,7 +2969,7 @@ Library { ";'on';'on';'on';'on'});\nend\nclear subOrFull " } Object { - $ObjectID 106 + $ObjectID 129 Type "edit" Name "controlledJoints" Prompt "Controlled Joint List" @@ -2789,7 +2977,7 @@ Library { Visible "off" } Object { - $ObjectID 107 + $ObjectID 130 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -2797,7 +2985,7 @@ Library { Tunable "off" } Object { - $ObjectID 108 + $ObjectID 131 Type "edit" Name "localName" Prompt "Model Name" @@ -2805,7 +2993,7 @@ Library { Tunable "off" } Object { - $ObjectID 109 + $ObjectID 132 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -2813,7 +3001,7 @@ Library { Tunable "off" } Object { - $ObjectID 110 + $ObjectID 133 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -2826,11 +3014,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 111 + $ObjectID 134 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 112 + $ObjectID 135 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -2838,33 +3026,33 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 113 + $ObjectID 136 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 114 + $ObjectID 137 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 115 + $ObjectID 138 Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 116 + $ObjectID 139 $ClassName "Simulink.dialog.parameter.Popup" Name "controlType" } Object { - $ObjectID 117 + $ObjectID 140 $ClassName "Simulink.dialog.parameter.Popup" Name "fullOrSubControl" } Object { - $ObjectID 118 + $ObjectID 141 $ClassName "Simulink.dialog.parameter.Edit" Name "controlledJoints" } @@ -2873,25 +3061,25 @@ Library { Name "Container8" } Object { - $ObjectID 119 + $ObjectID 142 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 120 + $ObjectID 143 Name "robotName" } Object { - $ObjectID 121 + $ObjectID 144 Name "localName" } Object { - $ObjectID 122 + $ObjectID 145 Name "wbiFile" } Object { - $ObjectID 123 + $ObjectID 146 Name "wbiList" } PropName "DialogControls" @@ -2968,7 +3156,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 124 + $ObjectID 147 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyModel.png'),'center')" } @@ -2999,7 +3187,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 125 + $ObjectID 148 $ClassName "Simulink.Mask" Display "image(imread('Dynamics.png'))" } @@ -3033,7 +3221,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 126 + $ObjectID 149 $ClassName "Simulink.Mask" Type "Centroidal Momentum" Description "This block computed the 6 element centroidal momentum, as defined in:\n\"Centroidal dynamics of a huma" @@ -3052,7 +3240,7 @@ Library { Type "Simulink.MaskParameter" Dimension 4 Object { - $ObjectID 127 + $ObjectID 150 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -3060,7 +3248,7 @@ Library { Tunable "off" } Object { - $ObjectID 128 + $ObjectID 151 Type "edit" Name "localName" Prompt "Model Name" @@ -3068,7 +3256,7 @@ Library { Tunable "off" } Object { - $ObjectID 129 + $ObjectID 152 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -3076,7 +3264,7 @@ Library { Tunable "off" } Object { - $ObjectID 130 + $ObjectID 153 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -3209,7 +3397,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 131 + $ObjectID 154 $ClassName "Simulink.Mask" Type "Get Generalized Bias Forces" Description "This block retrieves the generalied bias forces of the robot.\n\nParameter:\n- Explicit Gravity. If Tr" @@ -3234,7 +3422,7 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 132 + $ObjectID 155 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -3242,7 +3430,7 @@ Library { Tunable "off" } Object { - $ObjectID 133 + $ObjectID 156 Type "edit" Name "localName" Prompt "Model Name" @@ -3250,7 +3438,7 @@ Library { Tunable "off" } Object { - $ObjectID 134 + $ObjectID 157 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -3258,7 +3446,7 @@ Library { Tunable "off" } Object { - $ObjectID 135 + $ObjectID 158 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -3266,7 +3454,7 @@ Library { Tunable "off" } Object { - $ObjectID 136 + $ObjectID 159 Type "checkbox" Name "explicitGravity" Prompt "Explicit Gravity" @@ -3444,7 +3632,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 137 + $ObjectID 160 $ClassName "Simulink.Mask" Type "Gravity bias" Description "This block compute the generalized bias forces due to the gravity\n\nParameter:\n- Explicit Gravity. I" @@ -3467,7 +3655,7 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 138 + $ObjectID 161 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -3475,7 +3663,7 @@ Library { Tunable "off" } Object { - $ObjectID 139 + $ObjectID 162 Type "edit" Name "localName" Prompt "Model Name" @@ -3483,7 +3671,7 @@ Library { Tunable "off" } Object { - $ObjectID 140 + $ObjectID 163 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -3491,7 +3679,7 @@ Library { Tunable "off" } Object { - $ObjectID 141 + $ObjectID 164 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -3499,7 +3687,7 @@ Library { Tunable "off" } Object { - $ObjectID 142 + $ObjectID 165 Type "checkbox" Name "explicitGravity" Prompt "Explicit Gravity" @@ -3664,7 +3852,7 @@ Library { SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 143 + $ObjectID 166 $ClassName "Simulink.Mask" Type "Inverse Dynamics" Description "This block compute the inverse dynamics of the robot.\n\nParameter:\n- Explicit Gravity. If True, an a" @@ -3690,35 +3878,35 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 144 + $ObjectID 167 Type "edit" Name "robotName" Prompt "Robot Port Name" Value "WBT_robotName" } Object { - $ObjectID 145 + $ObjectID 168 Type "edit" Name "localName" Prompt "Model Name" Value "WBT_modelName" } Object { - $ObjectID 146 + $ObjectID 169 Type "edit" Name "wbiFile" Prompt "WBI filename" Value "WBT_wbiFilename" } Object { - $ObjectID 147 + $ObjectID 170 Type "edit" Name "wbiList" Prompt "WBI list name" Value "WBT_wbiList" } Object { - $ObjectID 148 + $ObjectID 171 Type "checkbox" Name "explicitGravity" Prompt "Explicit Gravity" @@ -3743,7 +3931,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 149 + $ObjectID 172 $ClassName "Simulink.Mask" Type "Mass Matrix" Description "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of degrees of freedom of th" @@ -3759,7 +3947,7 @@ Library { Type "Simulink.MaskParameter" Dimension 4 Object { - $ObjectID 150 + $ObjectID 173 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -3767,7 +3955,7 @@ Library { Tunable "off" } Object { - $ObjectID 151 + $ObjectID 174 Type "edit" Name "localName" Prompt "Model Name" @@ -3775,7 +3963,7 @@ Library { Tunable "off" } Object { - $ObjectID 152 + $ObjectID 175 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -3783,7 +3971,7 @@ Library { Tunable "off" } Object { - $ObjectID 153 + $ObjectID 176 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -3885,7 +4073,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 154 + $ObjectID 177 $ClassName "Simulink.Mask" Display "image(imread('jacobian.png'))" } @@ -3919,7 +4107,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 155 + $ObjectID 178 $ClassName "Simulink.Mask" Type "DotJ Nu" Description "This block computes the product between the time derivative of the\n Jacobian of the specified" @@ -3943,14 +4131,14 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 156 + $ObjectID 179 Type "edit" Name "frameName" Prompt "Frame name" Value "'frame'" } Object { - $ObjectID 157 + $ObjectID 180 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -3958,7 +4146,7 @@ Library { Tunable "off" } Object { - $ObjectID 158 + $ObjectID 181 Type "edit" Name "localName" Prompt "Model Name" @@ -3966,7 +4154,7 @@ Library { Tunable "off" } Object { - $ObjectID 159 + $ObjectID 182 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -3974,7 +4162,7 @@ Library { Tunable "off" } Object { - $ObjectID 160 + $ObjectID 183 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -3987,11 +4175,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 161 + $ObjectID 184 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 162 + $ObjectID 185 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -3999,46 +4187,46 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 163 + $ObjectID 186 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 164 + $ObjectID 187 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 165 + $ObjectID 188 Prompt "Block parameters" Object { $PropName "DialogControls" - $ObjectID 166 + $ObjectID 189 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Name "Container8" } Object { - $ObjectID 167 + $ObjectID 190 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 168 + $ObjectID 191 Name "robotName" } Object { - $ObjectID 169 + $ObjectID 192 Name "localName" } Object { - $ObjectID 170 + $ObjectID 193 Name "wbiFile" } Object { - $ObjectID 171 + $ObjectID 194 Name "wbiList" } PropName "DialogControls" @@ -4178,7 +4366,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 172 + $ObjectID 195 $ClassName "Simulink.Mask" Type "Jacobian" Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of degrees of" @@ -4198,14 +4386,14 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 173 + $ObjectID 196 Type "edit" Name "frameName" Prompt "Frame name" Value "'frame'" } Object { - $ObjectID 174 + $ObjectID 197 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -4213,7 +4401,7 @@ Library { Tunable "off" } Object { - $ObjectID 175 + $ObjectID 198 Type "edit" Name "localName" Prompt "Model Name" @@ -4221,7 +4409,7 @@ Library { Tunable "off" } Object { - $ObjectID 176 + $ObjectID 199 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -4229,7 +4417,7 @@ Library { Tunable "off" } Object { - $ObjectID 177 + $ObjectID 200 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -4242,11 +4430,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 178 + $ObjectID 201 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 179 + $ObjectID 202 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4254,46 +4442,46 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 180 + $ObjectID 203 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 181 + $ObjectID 204 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 182 + $ObjectID 205 Prompt "Block parameters" Object { $PropName "DialogControls" - $ObjectID 183 + $ObjectID 206 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Name "Container8" } Object { - $ObjectID 184 + $ObjectID 207 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 185 + $ObjectID 208 Name "robotName" } Object { - $ObjectID 186 + $ObjectID 209 Name "localName" } Object { - $ObjectID 187 + $ObjectID 210 Name "wbiFile" } Object { - $ObjectID 188 + $ObjectID 211 Name "wbiList" } PropName "DialogControls" @@ -4401,7 +4589,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 189 + $ObjectID 212 $ClassName "Simulink.Mask" Display "image(imread('forwardKinematics.png'))" } @@ -4435,7 +4623,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 190 + $ObjectID 213 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4455,14 +4643,14 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 191 + $ObjectID 214 Type "edit" Name "frameName" Prompt "Frame name" Value "'frame'" } Object { - $ObjectID 192 + $ObjectID 215 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -4470,7 +4658,7 @@ Library { Tunable "off" } Object { - $ObjectID 193 + $ObjectID 216 Type "edit" Name "localName" Prompt "Model Name" @@ -4478,7 +4666,7 @@ Library { Tunable "off" } Object { - $ObjectID 194 + $ObjectID 217 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -4486,7 +4674,7 @@ Library { Tunable "off" } Object { - $ObjectID 195 + $ObjectID 218 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -4499,11 +4687,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 196 + $ObjectID 219 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 197 + $ObjectID 220 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4511,46 +4699,46 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 198 + $ObjectID 221 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 199 + $ObjectID 222 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 200 + $ObjectID 223 Prompt "Block parameters" Object { $PropName "DialogControls" - $ObjectID 201 + $ObjectID 224 $ClassName "Simulink.dialog.parameter.Edit" Name "frameName" } Name "Container8" } Object { - $ObjectID 202 + $ObjectID 225 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 203 + $ObjectID 226 Name "robotName" } Object { - $ObjectID 204 + $ObjectID 227 Name "localName" } Object { - $ObjectID 205 + $ObjectID 228 Name "wbiFile" } Object { - $ObjectID 206 + $ObjectID 229 Name "wbiList" } PropName "DialogControls" @@ -4659,7 +4847,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 207 + $ObjectID 230 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4683,21 +4871,21 @@ Library { Type "Simulink.MaskParameter" Dimension 7 Object { - $ObjectID 208 + $ObjectID 231 Type "edit" Name "baseFrame" Prompt "Base Frame" Value "'root_link'" } Object { - $ObjectID 209 + $ObjectID 232 Type "edit" Name "endEffFrame" Prompt "End Effector frame" Value "'l_sole'" } Object { - $ObjectID 210 + $ObjectID 233 Type "popup" Array { Type "Cell" @@ -4711,7 +4899,7 @@ Library { Value "Full Constraint (Position and Orientation)" } Object { - $ObjectID 211 + $ObjectID 234 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -4719,7 +4907,7 @@ Library { Tunable "off" } Object { - $ObjectID 212 + $ObjectID 235 Type "edit" Name "localName" Prompt "Model Name" @@ -4727,7 +4915,7 @@ Library { Tunable "off" } Object { - $ObjectID 213 + $ObjectID 236 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -4735,7 +4923,7 @@ Library { Tunable "off" } Object { - $ObjectID 214 + $ObjectID 237 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -4748,11 +4936,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 215 + $ObjectID 238 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 216 + $ObjectID 239 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -4760,34 +4948,34 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 217 + $ObjectID 240 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 218 + $ObjectID 241 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 219 + $ObjectID 242 Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 220 + $ObjectID 243 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "baseFrame" } Object { - $ObjectID 221 + $ObjectID 244 $ClassName "Simulink.dialog.parameter.Edit" Name "endEffFrame" } Object { - $ObjectID 222 + $ObjectID 245 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -4796,25 +4984,25 @@ Library { Name "Container8" } Object { - $ObjectID 223 + $ObjectID 246 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 224 + $ObjectID 247 Name "robotName" } Object { - $ObjectID 225 + $ObjectID 248 Name "localName" } Object { - $ObjectID 226 + $ObjectID 249 Name "wbiFile" } Object { - $ObjectID 227 + $ObjectID 250 Name "wbiList" } PropName "DialogControls" @@ -4953,7 +5141,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 228 + $ObjectID 251 $ClassName "Simulink.Mask" Type "Forward Kinematics" Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " @@ -4979,14 +5167,14 @@ Library { Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 229 + $ObjectID 252 Type "edit" Name "solverName" Prompt "Solver Name" Value "'/cartesianSolver'" } Object { - $ObjectID 230 + $ObjectID 253 Type "edit" Name "dofs" Prompt "#Dofs" @@ -4994,7 +5182,7 @@ Library { Tunable "off" } Object { - $ObjectID 231 + $ObjectID 254 Type "popup" Array { Type "Cell" @@ -5013,11 +5201,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 232 + $ObjectID 255 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 233 + $ObjectID 256 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5025,33 +5213,33 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 234 + $ObjectID 257 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 235 + $ObjectID 258 $ClassName "Simulink.dialog.TabContainer" Object { $PropName "DialogControls" - $ObjectID 236 + $ObjectID 259 $ClassName "Simulink.dialog.Tab" Prompt "Block parameters" Array { Type "Simulink.dialog.parameter.Control" Dimension 3 Object { - $ObjectID 237 + $ObjectID 260 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" Name "solverName" } Object { - $ObjectID 238 + $ObjectID 261 $ClassName "Simulink.dialog.parameter.Edit" Name "dofs" } Object { - $ObjectID 239 + $ObjectID 262 $ClassName "Simulink.dialog.parameter.Popup" Name "optOption" } @@ -5159,7 +5347,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 240 + $ObjectID 263 $ClassName "Simulink.Mask" Display "image(imread('wholeBodyStates.png'),'center');" } @@ -5193,7 +5381,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 241 + $ObjectID 264 $ClassName "Simulink.Mask" Type "Get Estimate" Description "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of " @@ -5208,7 +5396,7 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 242 + $ObjectID 265 Type "popup" Array { Type "Cell" @@ -5231,7 +5419,7 @@ Library { "');\nend\nclear maskStr" } Object { - $ObjectID 243 + $ObjectID 266 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -5239,7 +5427,7 @@ Library { Tunable "off" } Object { - $ObjectID 244 + $ObjectID 267 Type "edit" Name "localName" Prompt "Model Name" @@ -5247,7 +5435,7 @@ Library { Tunable "off" } Object { - $ObjectID 245 + $ObjectID 268 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -5255,7 +5443,7 @@ Library { Tunable "off" } Object { - $ObjectID 246 + $ObjectID 269 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -5268,11 +5456,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 247 + $ObjectID 270 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 248 + $ObjectID 271 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5280,46 +5468,46 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 249 + $ObjectID 272 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 250 + $ObjectID 273 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 251 + $ObjectID 274 Prompt "Block parameters" Object { $PropName "DialogControls" - $ObjectID 252 + $ObjectID 275 $ClassName "Simulink.dialog.parameter.Popup" Name "estimateType" } Name "Container8" } Object { - $ObjectID 253 + $ObjectID 276 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 254 + $ObjectID 277 Name "robotName" } Object { - $ObjectID 255 + $ObjectID 278 Name "localName" } Object { - $ObjectID 256 + $ObjectID 279 Name "wbiFile" } Object { - $ObjectID 257 + $ObjectID 280 Name "wbiList" } PropName "DialogControls" @@ -5395,7 +5583,7 @@ Library { Variant off Object { $PropName "MaskObject" - $ObjectID 258 + $ObjectID 281 $ClassName "Simulink.Mask" Type "Get Estimate" Description "This block get the estimate of the specified type\n\nAssuming DoFs is the number of degrees of " @@ -5410,7 +5598,7 @@ Library { Type "Simulink.MaskParameter" Dimension 5 Object { - $ObjectID 259 + $ObjectID 282 Type "popup" Array { Type "Cell" @@ -5430,7 +5618,7 @@ Library { ";\nend\nclear maskStr" } Object { - $ObjectID 260 + $ObjectID 283 Type "edit" Name "robotName" Prompt "Robot Port Name" @@ -5438,7 +5626,7 @@ Library { Tunable "off" } Object { - $ObjectID 261 + $ObjectID 284 Type "edit" Name "localName" Prompt "Model Name" @@ -5446,7 +5634,7 @@ Library { Tunable "off" } Object { - $ObjectID 262 + $ObjectID 285 Type "edit" Name "wbiFile" Prompt "WBI filename" @@ -5454,7 +5642,7 @@ Library { Tunable "off" } Object { - $ObjectID 263 + $ObjectID 286 Type "edit" Name "wbiList" Prompt "WBI list name" @@ -5467,11 +5655,11 @@ Library { Type "Simulink.dialog.Group" Dimension 2 Object { - $ObjectID 264 + $ObjectID 287 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 265 + $ObjectID 288 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -5479,46 +5667,46 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 266 + $ObjectID 289 Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 267 + $ObjectID 290 $ClassName "Simulink.dialog.TabContainer" Array { Type "Simulink.dialog.Tab" Dimension 2 Object { - $ObjectID 268 + $ObjectID 291 Prompt "Block parameters" Object { $PropName "DialogControls" - $ObjectID 269 + $ObjectID 292 $ClassName "Simulink.dialog.parameter.Popup" Name "limitsType" } Name "Container8" } Object { - $ObjectID 270 + $ObjectID 293 Prompt "WBI parameters" Array { Type "Simulink.dialog.parameter.Edit" Dimension 4 Object { - $ObjectID 271 + $ObjectID 294 Name "robotName" } Object { - $ObjectID 272 + $ObjectID 295 Name "localName" } Object { - $ObjectID 273 + $ObjectID 296 Name "wbiFile" } Object { - $ObjectID 274 + $ObjectID 297 Name "wbiList" } PropName "DialogControls" @@ -5600,7 +5788,7 @@ Library { Annotation { SID "1213" Name "WHOLE BODY TOOLBOX" - Position [172, 149, 311, 166] + Position [172, 149, 317, 165] InternalMargins [0, 0, 0, 0] FixedHeight off FixedWidth off @@ -5615,7 +5803,7 @@ Library { Name "Copyright 2013-2016 iCub Facility Department, Istituto Italiano di Tecnologia\nThis software was s" "upported by the FP7 EU Project CoDyCO (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b))\nhttp://www.c" "odyco.eu" - Position [102, 184, 379, 201] + Position [102, 184, 380, 204] InternalMargins [0, 0, 0, 0] FixedHeight off FixedWidth off diff --git a/toolbox/autogenerated/CMakeLists.txt b/toolbox/autogenerated/CMakeLists.txt new file mode 100644 index 000000000..3ce96c1c5 --- /dev/null +++ b/toolbox/autogenerated/CMakeLists.txt @@ -0,0 +1,38 @@ +cmake_policy(SET CMP0048 NEW) +project(ClockServer LANGUAGES CXX VERSION 0.1) +cmake_minimum_required(VERSION 3.0.2) + +# Configure the project +# ===================== +set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +include(GNUInstallDirs) +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# Use a general prefix for the project +set(VARS_PREFIX ${PROJECT_NAME}) + +# Build the library +# ================= + +# Export the includes needed to who inherits this library +# Set this up properly for handling either BUILD and INSTALL trees +set(${VARS_PREFIX}_INCLUDE_BUILD ${CMAKE_CURRENT_SOURCE_DIR}/include) +set(${VARS_PREFIX}_INCLUDE_INSTALL ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX}) + +# Add the target +add_library(${VARS_PREFIX} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/include/thrift/ClockServer.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/thrift/ClockServer.cpp) + +if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") + set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") +endif() + +# Setup the include directories +target_include_directories(${VARS_PREFIX} PUBLIC + $ + $) + +# YARP +find_package(YARP REQUIRED) +target_include_directories(${VARS_PREFIX} PUBLIC ${YARP_INCLUDE_DIRS}) diff --git a/toolbox/autogenerated/src/thrift/ClockServer.cpp b/toolbox/autogenerated/src/thrift/ClockServer.cpp index 30ced2c9d..64d4cf3e8 100644 --- a/toolbox/autogenerated/src/thrift/ClockServer.cpp +++ b/toolbox/autogenerated/src/thrift/ClockServer.cpp @@ -1,7 +1,7 @@ // This is an automatically-generated file. // It could get re-generated if the ALLOW_IDL_GENERATION flag is on. -#include +#include "thrift/ClockServer.h" #include namespace gazebo { @@ -382,5 +382,3 @@ std::vector ClockServer::help(const std::string& functionName) { return helpString; } } // namespace - - diff --git a/toolbox/include/CentroidalMomentum.h b/toolbox/include/CentroidalMomentum.h index c4c5c121d..bfb620a60 100644 --- a/toolbox/include/CentroidalMomentum.h +++ b/toolbox/include/CentroidalMomentum.h @@ -1,35 +1,39 @@ #ifndef WBT_CENTROIDALMOMENTUM_H #define WBT_CENTROIDALMOMENTUM_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include namespace wbt { class CentroidalMomentum; } -class wbt::CentroidalMomentum : public wbt::WBIModelBlock { +namespace iDynTree { + class SpatialMomentum; +} - double *m_basePose; - double *m_centroidalMomentum; +class wbt::CentroidalMomentum : public wbt::WBBlock +{ +private: + std::unique_ptr m_centroidalMomentum; - //input buffers - double *m_basePoseRaw; - double *m_configuration; - double *m_baseVelocity; - double *m_jointsVelocity; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned INPUT_IDX_BASE_VEL; + static const unsigned INPUT_IDX_JOINT_VEL; + static const unsigned OUTPUT_IDX_CENTRMOM; public: - static std::string ClassName; + static const std::string ClassName; + CentroidalMomentum(); + ~CentroidalMomentum() override = default; - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_CENTROIDALMOMENTUM_H */ +#endif /* WBT_CENTROIDALMOMENTUM_H */ diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h new file mode 100644 index 000000000..65915f3a1 --- /dev/null +++ b/toolbox/include/DiscreteFilter.h @@ -0,0 +1,65 @@ +#include "Block.h" +#include +#include +#include + +#ifndef WBT_FILTER_H +#define WBT_FILTER_H + +namespace wbt { + class DiscreteFilter; +} // namespace wbt + +namespace iCub { + namespace ctrl { + class IFilter; + } +} // namespace iCub + +namespace yarp { + namespace sig { + class Vector; + } +} // namespace yarp + +class wbt::DiscreteFilter : public wbt::Block { +private: + unsigned inputSignalWidth; + std::unique_ptr filter; + std::unique_ptr y0; + std::unique_ptr u0; + std::unique_ptr inputSignalVector; + + static void stringToYarpVector(const std::string s, yarp::sig::Vector& v); + + // Parameters + static const unsigned PARAM_IDX_FLT_TYPE; + static const unsigned PARAM_IDX_NUMCOEFF; + static const unsigned PARAM_IDX_DENCOEFF; + static const unsigned PARAM_IDX_1LOWP_FC; + static const unsigned PARAM_IDX_1LOWP_TS; + static const unsigned PARAM_IDX_MD_ORDER; + static const unsigned PARAM_IDX_INIT_Y0; + static const unsigned PARAM_IDX_INIT_U0; + // Inputs + static const unsigned INPUT_IDX_SIGNAL; + // Outputs + static const unsigned OUTPUT_IDX_SIGNAL; + // Other defines + static const int SIGNAL_DYNAMIC_SIZE; + +public: + static const std::string ClassName; + + DiscreteFilter(); + ~DiscreteFilter() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool initializeInitialConditions(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; +}; + +#endif // WBT_FILTER_H diff --git a/toolbox/include/DotJNu.h b/toolbox/include/DotJNu.h index 343b8b15c..0bc8153ee 100644 --- a/toolbox/include/DotJNu.h +++ b/toolbox/include/DotJNu.h @@ -1,38 +1,43 @@ #ifndef WBT_DOTJDOTQ_H #define WBT_DOTJDOTQ_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include +#include +#include namespace wbt { class DotJNu; } -class wbt::DotJNu : public wbt::WBIModelBlock { +class wbt::DotJNu : public wbt::WBBlock +{ +private: + // Output + std::unique_ptr m_dotJNu; - double *m_basePose; - double *m_dotJNu; + // Other variables + bool m_frameIsCoM; + iDynTree::FrameIndex m_frameIndex; - //input buffers - double *m_basePoseRaw; - double *m_configuration; - double *m_baseVelocity; - double *m_jointsVelocity; - - int m_frameIndex; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned INPUT_IDX_BASE_VEL; + static const unsigned INPUT_IDX_JOINT_VEL; + static const unsigned OUTPUT_IDX_DOTJ_NU; public: - static std::string ClassName; + static const std::string ClassName; + DotJNu(); + ~DotJNu() override = default; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_DOTJDOTQ_H */ +#endif /* WBT_DOTJDOTQ_H */ diff --git a/toolbox/include/ForwardKinematics.h b/toolbox/include/ForwardKinematics.h index 6ed0e5bf9..4cf69167a 100644 --- a/toolbox/include/ForwardKinematics.h +++ b/toolbox/include/ForwardKinematics.h @@ -1,36 +1,36 @@ #ifndef WBT_FORWARDKINEMATICS_H #define WBT_FORWARDKINEMATICS_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include +#include namespace wbt { class ForwardKinematics; } -class wbt::ForwardKinematics : public wbt::WBIModelBlock { +class wbt::ForwardKinematics : public wbt::WBBlock +{ +private: + bool m_frameIsCoM; + iDynTree::FrameIndex m_frameIndex; - double *m_basePose; - double *m_frameForwardKinematics; - - //input buffers - double *m_basePoseRaw; - double *m_configuration; - - int m_frameIndex; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned OUTPUT_IDX_FW_FRAME; public: - static std::string ClassName; - ForwardKinematics(); + static const std::string ClassName; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + ForwardKinematics(); + ~ForwardKinematics() override = default; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_FORWARDKINEMATICS_H */ +#endif /* WBT_FORWARDKINEMATICS_H */ diff --git a/toolbox/include/GetEstimate.h b/toolbox/include/GetEstimate.h deleted file mode 100644 index a376d1484..000000000 --- a/toolbox/include/GetEstimate.h +++ /dev/null @@ -1,31 +0,0 @@ -#ifndef WBT_GETESTIMATE_H -#define WBT_GETESTIMATE_H - -#include "WBIBlock.h" -#include - -namespace wbt { - class GetEstimate; -} - -class wbt::GetEstimate : public wbt::WBIBlock { - - double *m_estimate; - wbi::EstimateType m_estimateType; - -public: - static std::string ClassName; - GetEstimate(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - -}; - - -#endif /* end of include guard: WBT_GETESTIMATE_H */ diff --git a/toolbox/include/GetLimits.h b/toolbox/include/GetLimits.h index 539386a67..e96a7ae59 100644 --- a/toolbox/include/GetLimits.h +++ b/toolbox/include/GetLimits.h @@ -1,30 +1,43 @@ #ifndef WBT_GETLIMITS_H #define WBT_GETLIMITS_H -#include "WBIBlock.h" +#include "WBBlock.h" +#include namespace wbt { class GetLimits; + struct Limit; } -class wbt::GetLimits : public wbt::WBIBlock { +struct wbt::Limit +{ + std::vector min; + std::vector max; - struct Limit; - struct Limit *m_limits; - -public: - static std::string ClassName; - GetLimits(); + Limit(unsigned size = 0) + : min(size) + , max(size) + {} +}; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); +class wbt::GetLimits : public wbt::WBBlock +{ +private: + std::unique_ptr m_limits; + static double deg2rad(const double& v); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); +public: + static const std::string ClassName; + GetLimits() = default; + ~GetLimits() override = default; -}; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; +}; -#endif /* end of include guard: WBT_GETLIMITS_H */ +#endif /* WBT_GETLIMITS_H */ diff --git a/toolbox/include/GetMeasurement.h b/toolbox/include/GetMeasurement.h new file mode 100644 index 000000000..5caec2f71 --- /dev/null +++ b/toolbox/include/GetMeasurement.h @@ -0,0 +1,38 @@ +#ifndef WBT_GETMEASUREMENT_H +#define WBT_GETMEASUREMENT_H + +#include "WBBlock.h" + +namespace wbt { + class GetMeasurement; + enum MeasuredType { + MEASUREMENT_JOINT_POS, + MEASUREMENT_JOINT_VEL, + MEASUREMENT_JOINT_ACC, + ESTIMATE_JOINT_TORQUE + }; +} + + +class wbt::GetMeasurement : public wbt::WBBlock +{ +private: + std::vector m_measurement; + wbt::MeasuredType m_measuredType; + static void deg2rad(std::vector& v); + +public: + static const std::string ClassName; + + GetMeasurement() = default; + ~GetMeasurement() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; +}; + +#endif /* end of include guard: WBT_GETMEASUREMENT_H */ diff --git a/toolbox/include/InverseDynamics.h b/toolbox/include/InverseDynamics.h index 58e796131..afb1d2ca5 100644 --- a/toolbox/include/InverseDynamics.h +++ b/toolbox/include/InverseDynamics.h @@ -1,41 +1,49 @@ #ifndef WBT_INVERSEDYNAMICS_H #define WBT_INVERSEDYNAMICS_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include +#include namespace wbt { class InverseDynamics; } -class wbt::InverseDynamics : public wbt::WBIModelBlock { +namespace iDynTree { + class VectorDynSize; + class FreeFloatingGeneralizedTorques; +} - double *m_basePose; - double *m_torques; +class wbt::InverseDynamics : public wbt::WBBlock +{ +private: + std::unique_ptr m_baseAcceleration; + std::unique_ptr m_jointsAcceleration; - bool m_explicitGravity; + // Output + std::unique_ptr m_torques; - //input buffers - double *m_basePoseRaw; - double *m_configuration; - double *m_baseVelocity; - double *m_jointsVelocity; - double *m_baseAcceleration; - double *m_jointsAcceleration; - double m_gravity[3]; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned INPUT_IDX_BASE_VEL; + static const unsigned INPUT_IDX_JOINT_VEL; + static const unsigned INPUT_IDX_BASE_ACC; + static const unsigned INPUT_IDX_JOINT_ACC; + static const unsigned OUTPUT_IDX_TORQUES; public: - static std::string ClassName; + static const std::string ClassName; + InverseDynamics(); + ~InverseDynamics() override = default; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - - -}; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; +}; -#endif /* end of include guard: WBT_INVERSEDYNAMICS_H */ +#endif /* WBT_INVERSEDYNAMICS_H */ diff --git a/toolbox/include/Jacobian.h b/toolbox/include/Jacobian.h index 0c5307a19..a38a968c6 100644 --- a/toolbox/include/Jacobian.h +++ b/toolbox/include/Jacobian.h @@ -1,36 +1,46 @@ #ifndef WBT_JACOBIAN_H #define WBT_JACOBIAN_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include +#include namespace wbt { class Jacobian; } -class wbt::Jacobian : public wbt::WBIModelBlock { +namespace iDynTree { + class MatrixDynSize; +} + +class wbt::Jacobian : public wbt::WBBlock +{ +private: + // Support variables + std::unique_ptr m_jacobianCOM; - double *m_basePose; - double *m_jacobian; + // Output + std::unique_ptr m_jacobian; - //input buffers - double *m_basePoseRaw; - double *m_configuration; + // Other variables + bool m_frameIsCoM; + iDynTree::FrameIndex m_frameIndex; - int m_frameIndex; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned OUTPUT_IDX_FW_FRAME; public: - static std::string ClassName; + static const std::string ClassName; Jacobian(); + ~Jacobian() override = default; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_JACOBIAN_H */ +#endif /* WBT_JACOBIAN_H */ diff --git a/toolbox/include/MassMatrix.h b/toolbox/include/MassMatrix.h index ea0d58ce0..cd048c74d 100644 --- a/toolbox/include/MassMatrix.h +++ b/toolbox/include/MassMatrix.h @@ -1,33 +1,36 @@ #ifndef WBT_MASSMATRIX_H #define WBT_MASSMATRIX_H -#include "WBIModelBlock.h" +#include "WBBlock.h" +#include namespace wbt { class MassMatrix; } -class wbt::MassMatrix : public wbt::WBIModelBlock { +namespace iDynTree { + class MatrixDynSize; +} - double *m_basePose; - double *m_massMatrix; +class wbt::MassMatrix : public wbt::WBBlock +{ +private: + std::unique_ptr m_massMatrix; - //input buffers - double *m_basePoseRaw; - double *m_configuration; + static const unsigned INPUT_IDX_BASE_POSE; + static const unsigned INPUT_IDX_JOINTCONF; + static const unsigned OUTPUT_IDX_MASS_MAT; public: - static std::string ClassName; + static const std::string ClassName; MassMatrix(); + ~MassMatrix() override = default; - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; - -#endif /* end of include guard: WBT_MASSMATRIX_H */ +#endif /* WBT_MASSMATRIX_H */ diff --git a/toolbox/include/MinimumJerkTrajectoryGenerator.h b/toolbox/include/MinimumJerkTrajectoryGenerator.h index ba0415e9f..1e0250a25 100644 --- a/toolbox/include/MinimumJerkTrajectoryGenerator.h +++ b/toolbox/include/MinimumJerkTrajectoryGenerator.h @@ -1,4 +1,5 @@ #include "Block.h" +#include #ifndef WBT_MINJERKTRAJGENERATOR_H #define WBT_MINJERKTRAJGENERATOR_H @@ -22,19 +23,20 @@ namespace yarp { class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { public: - static std::string ClassName; - + static const std::string ClassName; + MinimumJerkTrajectoryGenerator(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + ~MinimumJerkTrajectoryGenerator() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; + private: - iCub::ctrl::minJerkTrajGen *m_generator; + std::unique_ptr m_generator; int m_outputFirstDerivativeIndex; int m_outputSecondDerivativeIndex; @@ -45,9 +47,18 @@ class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { bool m_explicitInitialValue; bool m_externalSettlingTime; bool m_resetOnSettlingTimeChange; - yarp::sig::Vector *m_initialValues; - yarp::sig::Vector *m_reference; - + std::unique_ptr m_initialValues; + std::unique_ptr m_reference; + + static const unsigned PARAM_IDX_SAMPLE_TIME; // Sample Time (double) + static const unsigned PARAM_IDX_SETTLING_TIME; // Settling Time (double) + static const unsigned PARAM_IDX_OUTPUT_1ST_DERIVATIVE; // Output first derivative (boolean) + static const unsigned PARAM_IDX_OUTPUT_2ND_DERIVATIVE; // Output second derivative (boolean) + static const unsigned PARAM_IDX_INITIAL_VALUE; // Initial signal value as input (boolean) + static const unsigned PARAM_IDX_EXT_SETTLINGTIME; // Control if the settling time comes from + // external port or static parameter + static const unsigned PARAM_IDX_RESET_CHANGEST; // True if the block should reset the traj + // generator in case settling time changes }; #endif /* end of include guard: WBT_MINJERKTRAJGENERATOR_H */ diff --git a/toolbox/include/ModelPartitioner.h b/toolbox/include/ModelPartitioner.h new file mode 100644 index 000000000..853c3f0c3 --- /dev/null +++ b/toolbox/include/ModelPartitioner.h @@ -0,0 +1,36 @@ +#ifndef WBT_MODELPARTITIONER_H +#define WBT_MODELPARTITIONER_H + +#include "WBBlock.h" +#include "RobotInterface.h" +#include +#include + +namespace wbt { + class ModelPartitioner; +} + +class wbt::ModelPartitioner : public wbt::WBBlock +{ +private: + bool m_yarp2WBI; + + std::shared_ptr m_jointsMapString; + std::shared_ptr m_controlledJointsMapCB; + std::shared_ptr m_controlBoardIdxLimit; + +public: + static const std::string ClassName; + + ModelPartitioner() = default; + ~ModelPartitioner() override = default; + + unsigned numberOfParameters() override; + + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; +}; + +#endif /* end of include guard: WBT_MODELPARTITIONER_H */ diff --git a/toolbox/include/RealTimeSynchronizer.h b/toolbox/include/RealTimeSynchronizer.h index 3438d727f..26a797589 100644 --- a/toolbox/include/RealTimeSynchronizer.h +++ b/toolbox/include/RealTimeSynchronizer.h @@ -7,24 +7,27 @@ namespace wbt { class RealTimeSynchronizer; } -class wbt::RealTimeSynchronizer : public wbt::Block { +class wbt::RealTimeSynchronizer : public wbt::Block +{ public: - static std::string ClassName; - + static const std::string ClassName; + RealTimeSynchronizer(); - virtual ~RealTimeSynchronizer(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + ~RealTimeSynchronizer() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; + private: double m_period; double m_initialTime; unsigned long m_counter; + + static const unsigned PARAM_PERIOD; // Period }; #endif /* end of include guard: WBT_REALTIMESYNCHRONIZER_H */ diff --git a/toolbox/include/SetLowLevelPID.h b/toolbox/include/SetLowLevelPID.h index e0d154a27..96b8384d6 100644 --- a/toolbox/include/SetLowLevelPID.h +++ b/toolbox/include/SetLowLevelPID.h @@ -1,65 +1,49 @@ #ifndef WBT_SETLOWLEVELPID_H_ #define WBT_SETLOWLEVELPID_H_ -#include "WBIBlock.h" -#include -#include +#include "WBBlock.h" +#include +#include namespace wbt { class SetLowLevelPID; -} - -namespace wbi { - class wholeBodyInterface; - class iWholeBodyActuators; -} - -namespace yarpWbi { - class PIDList; + enum PidDataIndex { + PGAIN = 0, + IGAIN = 1, + DGAIN = 2 + }; + typedef std::tuple PidData; } namespace yarp { - namespace os { - class Value; + namespace dev { + class Pid; } } -typedef std::map PidMap; - -class wbt::SetLowLevelPID : public wbt::WBIBlock { +class wbt::SetLowLevelPID : public wbt::WBBlock +{ +private: + std::vector m_appliedPidValues; + std::vector m_defaultPidValues; + std::unordered_map m_pidJointsFromParameters; - bool m_firstRun; - PidMap m_pids; - int m_lastGainSetIndex; - wbi::ControlMode m_controlMode; + yarp::dev::PidControlTypeEnum m_controlType; - bool loadLowLevelGainsFromFile(std::string filename, - const yarpWbi::PIDList &originalList, - wbi::wholeBodyInterface& interface, - yarpWbi::PIDList &loadedPIDs); - - bool loadGainsFromValue(const yarp::os::Value &gains, - PidMap &pidMap, - wbi::wholeBodyInterface& interface); - - bool setCurrentGains(const PidMap &pidMap, - std::string key, - wbi::iWholeBodyActuators& actuators); + bool readWBTPidConfigObject(const BlockInformation* blockInfo); public: - static std::string ClassName; - SetLowLevelPID(); + static const std::string ClassName; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + SetLowLevelPID() = default; + ~SetLowLevelPID() override = default; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; - + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; - - #endif /* end of include guard: WBT_SETLOWLEVELPID_H_ */ diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index ca3e1465f..16e2eba7a 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -1,36 +1,34 @@ #ifndef WBT_SETREFERENCES_H #define WBT_SETREFERENCES_H -#include "WBIBlock.h" -#include +#include "WBBlock.h" #include namespace wbt { class SetReferences; } -class wbt::SetReferences : public wbt::WBIBlock { - - double *m_references; - wbi::ControlMode m_controlMode; - bool m_fullControl; - std::vector m_controlledJoints; +class wbt::SetReferences : public wbt::WBBlock +{ +private: + std::vector m_controlModes; bool m_resetControlMode; + double m_refSpeed; + static const std::vector rad2deg(const double* buffer, const unsigned width); public: - static std::string ClassName; - SetReferences(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); + static const std::string ClassName; - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initializeInitialConditions(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + SetReferences(); + ~SetReferences() override = default; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool initializeInitialConditions(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; - #endif /* end of include guard: WBT_SETREFERENCES_H */ diff --git a/toolbox/include/SimulatorSynchronizer.h b/toolbox/include/SimulatorSynchronizer.h index 863fc6067..7b7793870 100644 --- a/toolbox/include/SimulatorSynchronizer.h +++ b/toolbox/include/SimulatorSynchronizer.h @@ -2,32 +2,37 @@ #define WBT_SIMULATORSYNCHRONIZER_H #include "Block.h" +#include namespace wbt { class SimulatorSynchronizer; } -class wbt::SimulatorSynchronizer : public wbt::Block { +class wbt::SimulatorSynchronizer : public wbt::Block +{ public: - static std::string ClassName; + static const std::string ClassName; SimulatorSynchronizer(); - virtual ~SimulatorSynchronizer(); + ~SimulatorSynchronizer() override = default; - virtual unsigned numberOfParameters(); - virtual std::vector additionalBlockOptions(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + unsigned numberOfParameters() override; + std::vector additionalBlockOptions() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; private: - double m_period; bool m_firstRun; struct RPCData; - RPCData *m_rpcData; + std::unique_ptr m_rpcData; + + static const unsigned PARAM_PERIOD; // Period + static const unsigned PARAM_GZCLK_PORT; // Gazebo clock port + static const unsigned PARAM_RPC_PORT; // RPC client port name }; #endif /* end of include guard: WBT_SIMULATORSYNCHRONIZER_H */ diff --git a/toolbox/include/YARPWBIConverter.h b/toolbox/include/YARPWBIConverter.h deleted file mode 100644 index 37f334057..000000000 --- a/toolbox/include/YARPWBIConverter.h +++ /dev/null @@ -1,29 +0,0 @@ -#ifndef WBT_YARP2DOFS_H -#define WBT_YARP2DOFS_H - -#include "WBIModelBlock.h" - -namespace wbt { - class YARPWBIConverter; -} - -class wbt::YARPWBIConverter : public wbt::WBIModelBlock { - - struct YARPWBIConverterPimpl; - YARPWBIConverterPimpl *m_pimpl; - -public: - static std::string ClassName; - YARPWBIConverter(); - - virtual unsigned numberOfParameters(); - - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - -}; - - -#endif /* end of include guard: WBT_YARP2DOFS_H */ diff --git a/toolbox/include/YarpClock.h b/toolbox/include/YarpClock.h index 9d0e551e5..430c78838 100644 --- a/toolbox/include/YarpClock.h +++ b/toolbox/include/YarpClock.h @@ -7,16 +7,19 @@ namespace wbt { class YarpClock; } -class wbt::YarpClock : public wbt::Block { +class wbt::YarpClock : public wbt::Block +{ public: - static std::string ClassName; + static const std::string ClassName; - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); + YarpClock() = default; + ~YarpClock() override = default; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; }; #endif /* end of include guard: WBT_YARPCLOCK_H */ diff --git a/toolbox/include/YarpRead.h b/toolbox/include/YarpRead.h index 14edd2564..33d9f6e64 100644 --- a/toolbox/include/YarpRead.h +++ b/toolbox/include/YarpRead.h @@ -2,6 +2,7 @@ #define WBT_YARPREAD_H #include "Block.h" +#include namespace wbt { class YarpRead; @@ -17,25 +18,34 @@ namespace yarp { } } -class wbt::YarpRead : public wbt::Block { +class wbt::YarpRead : public wbt::Block +{ public: - static std::string ClassName; - + static const std::string ClassName; + YarpRead(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + ~YarpRead() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; + private: bool m_autoconnect; bool m_blocking; bool m_shouldReadTimestamp; bool m_errorOnMissingPort; - - yarp::os::BufferedPort *m_port; + + std::unique_ptr> m_port; + + static const unsigned PARAM_IDX_PORTNAME; // port name + static const unsigned PARAM_IDX_PORTSIZE; // Size of the port you're reading + static const unsigned PARAM_IDX_WAITDATA; // boolean for blocking reading + static const unsigned PARAM_IDX_READ_TS; // boolean to stream timestamp + static const unsigned PARAM_IDX_AUTOCONNECT; // Autoconnect boolean + static const unsigned PARAM_IDX_ERR_NO_PORT; // Error on missing port if autoconnect is on boolean }; #endif /* end of include guard: WBT_YARPREAD_H */ diff --git a/toolbox/include/YarpWrite.h b/toolbox/include/YarpWrite.h index b7ea198ce..206567dff 100644 --- a/toolbox/include/YarpWrite.h +++ b/toolbox/include/YarpWrite.h @@ -2,6 +2,7 @@ #define WBT_YARPWRITE_H #include "Block.h" +#include namespace wbt { class YarpWrite; @@ -17,24 +18,30 @@ namespace yarp { } } -class wbt::YarpWrite : public wbt::Block { +class wbt::YarpWrite : public wbt::Block +{ public: - static std::string ClassName; - + static const std::string ClassName; + YarpWrite(); - - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); - virtual bool output(BlockInformation *blockInfo, wbt::Error *error); - + ~YarpWrite() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; + bool output(const BlockInformation* blockInfo) override; + private: bool m_autoconnect; bool m_errorOnMissingPort; std::string m_destinationPortName; - yarp::os::BufferedPort *m_port; + std::unique_ptr> m_port; + + static const unsigned PARAM_IDX_PORTNAME; // Port name + static const unsigned PARAM_IDX_AUTOCONNECT; // Autoconnect boolean + static const unsigned PARAM_IDX_ERR_NO_PORT; // Error on missing port if autoconnect is true }; #endif /* end of include guard: WBT_YARPWRITE_H */ diff --git a/toolbox/include/base/Block.h b/toolbox/include/base/Block.h index b65f52a7b..65f8447b6 100644 --- a/toolbox/include/base/Block.h +++ b/toolbox/include/base/Block.h @@ -6,7 +6,6 @@ namespace wbt { class Block; - class Error; class BlockInformation; } @@ -15,14 +14,15 @@ namespace wbt { * Basic abstract class for all the blocks. * This class (the whole toolbox in reality) assumes the block represent * an instantaneous system (i.e. not a dynamic system). - * + * * You can create a new block by deriving this class and implementing at least * all the pure virtual methods. * * @Note: if you need to implement a block which uses the WBI, you should derive * from WBIBlock as it already provides some facilities. */ -class wbt::Block { +class wbt::Block +{ public: /** * Create and returns a new Block object of the specified class. @@ -39,7 +39,7 @@ class wbt::Block { * */ virtual ~Block(); - + /** * Returns the number of configuration parameters needed by this block * @@ -59,7 +59,7 @@ class wbt::Block { * Returns the number of discrete states of the block. * * The base implementation returns 0, i.e. no discrete states - * @note if you return a number > 0, you should implement the + * @note if you return a number > 0, you should implement the * updateDiscreteState function * @return the number of discrete states */ @@ -82,7 +82,7 @@ class wbt::Block { * @param S the SimStruct structure * @return true for success, false otherwise */ - virtual bool updateDiscreteState(BlockInformation *blockInfo, wbt::Error *error); + virtual bool updateDiscreteState(const BlockInformation* blockInfo); /** * Not called for now @@ -90,7 +90,7 @@ class wbt::Block { * @param S the SimStruct structure * @return true for success, false otherwise */ - virtual bool stateDerivative(BlockInformation *blockInfo, wbt::Error *error); + virtual bool stateDerivative(const BlockInformation* blockInfo); /** @@ -111,21 +111,19 @@ class wbt::Block { * their size and configuration. * @Note: you should not save any object in this method because it will not persist * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. Check if the pointer exists before dereference it. * * @return true for success, false otherwise */ - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool configureSizeAndPorts(BlockInformation* blockInfo) = 0; /** * Never called. * * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. Check if the pointer exists before dereference it. * * @return true for success, false otherwise */ - virtual bool checkParameters(BlockInformation *blockInfo, wbt::Error *error); + virtual bool checkParameters(const BlockInformation* blockInfo); /** * Initialize the object for the simulation @@ -133,11 +131,10 @@ class wbt::Block { * This method is called at model startup (i.e. during mdlStart) * @Note: you can save and initialize your object in this method * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. * * @return true for success, false otherwise */ - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool initialize(const BlockInformation* blockInfo) = 0; /** * Called to initialize the initial conditions @@ -146,11 +143,10 @@ class wbt::Block { * @note this function is also called on a reset event * @note if you need to perform initialization only once, than implement initialize * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. * * @return true for success, false otherwise */ - virtual bool initializeInitialConditions(BlockInformation *blockInfo, wbt::Error *error); + virtual bool initializeInitialConditions(const BlockInformation* blockInfo); /** * Perform model cleanup. @@ -158,11 +154,10 @@ class wbt::Block { * This method is called at model termination (i.e. during mdlTerminate). * You should release all the resources you requested during the initialize method * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. * * @return true for success, false otherwise */ - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool terminate(const BlockInformation* blockInfo) = 0; @@ -175,7 +170,7 @@ class wbt::Block { * * @return true for success, false otherwise */ - virtual bool output(BlockInformation *blockInfo, wbt::Error *error) = 0; + virtual bool output(const BlockInformation* blockInfo) = 0; public: diff --git a/toolbox/include/base/BlockInformation.h b/toolbox/include/base/BlockInformation.h index b16461d89..43ef04306 100644 --- a/toolbox/include/base/BlockInformation.h +++ b/toolbox/include/base/BlockInformation.h @@ -1,8 +1,8 @@ #ifndef WBT_BLOCKINFORMATION_H #define WBT_BLOCKINFORMATION_H +#include "AnyType.h" #include -#include namespace wbt { class BlockInformation; @@ -20,58 +20,31 @@ namespace wbt { PortDataTypeBoolean, } PortDataType; - - struct Data { - private: - double buffer; - public: - inline double doubleData() const { return buffer; } - inline void doubleData(const double& data) { buffer = data; } - inline float floatData() const { return static_cast(buffer); } - inline void floatData(const float& data) { buffer = static_cast(data); } - - inline int8_t int8Data() const { return static_cast(buffer); } - inline void int8Data(const int8_t& data) { buffer = static_cast(data); } - inline uint8_t uint8Data() const { return static_cast(buffer); } - inline void uint8Data(const uint8_t& data) { buffer = static_cast(data); } - - inline int16_t int16Data() const { return static_cast(buffer); } - inline void int16Data(const int16_t& data) { buffer = static_cast(data); } - inline uint16_t uint16Data() const { return static_cast(buffer); } - inline void uint16Data(const uint16_t& data) { buffer = static_cast(data); } - - inline int32_t int32Data() const { return static_cast(buffer); } - inline void int32Data(const int32_t& data) { buffer = static_cast(data); } - inline uint32_t uint32Data() const { return static_cast(buffer); } - inline void uint32Data(const uint32_t& data) { buffer = static_cast(data); } - - inline bool booleanData() const { return static_cast(buffer); } - inline void booleanData(const bool& data) { buffer = static_cast(data); } - - friend Signal; - }; - extern const std::string BlockOptionPrioritizeOrder; } class wbt::BlockInformation { public: - virtual ~BlockInformation(); + BlockInformation() = default; + virtual ~BlockInformation() = default; - //Block Options methods + // BLOCK OPTIONS METHODS + // ===================== /** * Convert a block option from its Toolbox identifier to a specific implementation * - * @param [in] key identifier of the block option + * @param [in] key identifier of the block option * @param [out] option implementation-specific block option - * @return true if the option has been converted. False otherwise + * @return true if the option has been converted. False otherwise */ - virtual bool optionFromKey(const std::string& key, Data &option) const; + virtual bool optionFromKey(const std::string& key, double& option) const; - //Parameters methods + // PARAMETERS METHODS + // ================== + /** * Reads the parameter at the specified index and interpret it as a string * @@ -80,13 +53,17 @@ class wbt::BlockInformation { * * @return true if success, false otherwise */ - virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) = 0; - virtual Data getScalarParameterAtIndex(unsigned parameterIndex) = 0; + virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const = 0; + virtual bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const = 0; + virtual bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const = 0; + virtual bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const = 0; + virtual bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const = 0; + + // PORT INFORMATION SETTERS + // ======================== - //Port information methods virtual bool setNumberOfInputPorts(unsigned numberOfPorts) = 0; - virtual bool setNumberOfOuputPorts(unsigned numberOfPorts) = 0; - //If portSize == -1, then it is dynamic + virtual bool setNumberOfOutputPorts(unsigned numberOfPorts) = 0; virtual bool setInputPortVectorSize(unsigned portNumber, int portSize) = 0; virtual bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns) = 0; virtual bool setOutputPortVectorSize(unsigned portNumber, int portSize) = 0; @@ -103,12 +80,13 @@ class wbt::BlockInformation { virtual bool setInputPortType(unsigned portNumber, PortDataType portType) = 0; virtual bool setOutputPortType(unsigned portNumber, PortDataType portType) = 0; - //Port data - virtual unsigned getInputPortWidth(unsigned portNumber) = 0; - virtual unsigned getOutputPortWidth(unsigned portNumber) = 0; - virtual wbt::Signal getInputPortSignal(unsigned portNumber) = 0; - virtual wbt::Signal getOutputPortSignal(unsigned portNumber) = 0; + // PORT INFORMATION GETTERS + // ======================== + virtual unsigned getInputPortWidth(unsigned portNumber) const = 0; + virtual unsigned getOutputPortWidth(unsigned portNumber) const = 0; + virtual wbt::Signal getInputPortSignal(unsigned portNumber, int portWidth = -1) const = 0; + virtual wbt::Signal getOutputPortSignal(unsigned portNumber, int portWidth = -1) const = 0; }; #endif /* end of include guard: WBT_BLOCKINFORMATION_H */ diff --git a/toolbox/include/base/Configuration.h b/toolbox/include/base/Configuration.h new file mode 100644 index 000000000..4f0884a67 --- /dev/null +++ b/toolbox/include/base/Configuration.h @@ -0,0 +1,175 @@ +#ifndef WBT_TOOLBOXCONFIG_H +#define WBT_TOOLBOXCONFIG_H + +#include +#include +#include + +namespace wbt { + class Configuration; +} + +/** + * \class Configuration Configuration.h + * + * This class stores in a C++ object the content of the WBToolboxConfig Matlab class. + * + * @see WBToolboxConfig Matlab Class + * @see RobotInterface + */ +class wbt::Configuration +{ +// TODO: check how localName is used +private: + const std::string m_confKey; ///< Name of the block which this object refers to + std::string m_robotName; ///< Name of the robot + std::string m_urdfFile; ///< Name of the file containing the urdf model + std::string m_localName; ///< Prefix appended to the opened ports + std::vector m_controlledJoints; ///< Subset of controlled joints + std::vector m_controlBoardsNames; ///< Names of the used ControlBoard names + std::array m_gravityVector; ///< The gravity vector + size_t m_dofs; //< DoFs extracted my m_controlBoardsNames vector + +public: + Configuration() = delete; + Configuration(const std::string& confKey); + ~Configuration() = default; + + // SET METHODS + // =========== + + /** + * Initialize the Configuration object + * + * @param robotName Name of the robot + * @param urdfFile Name of the file containing the urdf model + * @param controlledJoints Subset of controlled joints + * @param controlBoardsNames Names of the used ControlBoard names + * @param localName Prefix appended to the opened ports + * @param gravityVector The gravity vector + */ + void setParameters(const std::string& robotName, + const std::string& urdfFile, + const std::vector& controlledJoints, + const std::vector& controlBoardsNames, + const std::string& localName, + const std::array& gravityVector); + + /** + * Set the name of the robot + * + * @param robotName Name of the robot + */ + void setRobotName(const std::string& robotName); + + /** + * Set the name of the file containing the urdf model + * + * @param urdfFile Name of the file containing the urdf model + */ + void setUrdfFile(const std::string& urdfFile); + + /** + * Set the subset of controlled joints + * @param controlledJoints Subset of controlled joints + */ + void setControlledJoints(const std::vector& controlledJoints); + + /** + * Set the names of the used ControlBoard names + * @param controlBoardsNames Names of the used ControlBoard names + */ + void setControlBoardsNames(const std::vector& controlBoardsNames); + + /** + * Set the prefix appended to the opened ports + * @param localName Prefix appended to the opened ports + */ + void setLocalName(const std::string& localName); + + /** + * Set the gravity vector + * @param gravityVector The gravity vector + */ + void setGravityVector(const std::array& gravityVector); + + // GET METHODS + // =========== + + /** + * Set the name of the robot + * + * @return Name of the robot + */ + const std::string& getRobotName() const; + + /** + * Set the name of the file containing the urdf model + * + * @return Name of the file containing the urdf model + */ + const std::string& getUrdfFile() const; + + /** + * Set the subset of controlled joints + * + * @return Subset of controlled joints + */ + const std::vector& getControlledJoints() const; + + /** + * Set the names of the used ControlBoard names + * + * @return Names of the used ControlBoard names + */ + const std::vector& getControlBoardsNames() const; + + /** + * Set the prefix appended to the opened ports. A leading "/" is always present. + * + * @return Prefix appended to the opened ports + */ + const std::string& getLocalName() const; + + /** + * Get a string with a unique identifier, generated from the name of the config + * block from Simulink. It might be useful when yarp ports must have a unique prefix + * (e.g. two RemoteControlBoardRemappers which share a ControlBoard) + * + * @return the unique identifier + * @see setLocalName + */ + const std::string getUniqueId() const; + + /** + * Set the gravity vector + * + * @return The gravity vector + */ + const std::array& getGravityVector() const; + + /** + * Get the configured number of DoFs + * + * @return The configured number of DoFs + */ + const size_t& getNumberOfDoFs() const; + + // OTHER METHODS + // ============= + + /** + * Checks if the stored configuration is valid, i.e. all the required fields have + * been saved successfully + * + * @return True if the configuration is valid + */ + bool isValid() const; + + // OPERATORS OVERLOADING + // ===================== + + bool operator==(const Configuration &config) const; +}; + +#endif // WBT_TOOLBOXCONFIG_H diff --git a/toolbox/include/base/Error.h b/toolbox/include/base/Error.h deleted file mode 100644 index f11dc413c..000000000 --- a/toolbox/include/base/Error.h +++ /dev/null @@ -1,19 +0,0 @@ -#ifndef WBT_ERROR_H -#define WBT_ERROR_H - -#include - -namespace wbt { - class Error; -} - -/** - * Basic Error class - */ -class wbt::Error { -public: - std::string message; /**< a string representing the error message */ -}; - - -#endif /* end of include guard: WBT_ERROR_H */ diff --git a/toolbox/include/base/Log.h b/toolbox/include/base/Log.h new file mode 100644 index 000000000..e3f9d6764 --- /dev/null +++ b/toolbox/include/base/Log.h @@ -0,0 +1,45 @@ +#ifndef WBT_LOG_H +#define WBT_LOG_H + +#include +#include + +namespace wbt { + class Log; +} + +/** + * Basic Log class + */ +class wbt::Log +{ +private: + std::vector errors; + std::vector warnings; + std::string prefix; + + static std::string serializeVectorString(std::vector v, const std::string& prefix=""); +public: + + static wbt::Log& getSingleton(); + + void error(const std::string& errorMessage); + void warning(const std::string& warningMessage); + + void errorAppend(const std::string& errorMessage); + void warningAppend(const std::string& warningMessage); + + void resetPrefix(); + void setPrefix(const std::string& prefixMessage); + + std::string getErrors() const; + std::string getWarnings() const; + + void clearErrors(); + void clearWarnings(); + + void clear(); +}; + + +#endif /* end of include guard: WBT_LOG_H */ diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h new file mode 100644 index 000000000..76d338de3 --- /dev/null +++ b/toolbox/include/base/RobotInterface.h @@ -0,0 +1,283 @@ +#ifndef WBT_ROBOTINTERFACE_H +#define WBT_ROBOTINTERFACE_H + +#include "Configuration.h" +#include +#include +#include +#include +#include +#include + +namespace yarp { + namespace dev { + class PolyDriver; + class IPositionControl; + class IPositionDirect; + class IVelocityControl; + class ITorqueControl; + class IPWMControl; + class IControlMode2; + class ICurrentControl; + class IEncoders; + class IControlLimits2; + class IPidControl; + } +} + +namespace iDynTree { + class KinDynComputations; +} + +namespace wbt { + class RobotInterface; + struct YarpInterfaces; + + typedef int jointIdx_yarp; + typedef int jointIdx_iDynTree; + typedef unsigned cb_idx; + typedef unsigned max_cb_idx; + typedef unsigned controlledJointIdxCB; + typedef std::unordered_map> JointsMapIndex; + typedef std::unordered_map> JointsMapString; + typedef std::unordered_map ControlledJointsMapCB; + typedef std::unordered_map ControlBoardIdxLimit; +} + +/** + * \struct wbt::YarpInterfaces RobotInterface.h + * + * This struct contains shared_ptrs to the devices which are (lazy) asked from the blocks. + * + * @note The shared_ptr is owned only by wbt::RobotInterface. All the blocks will receive a + * weak_ptr. + * @remark Right now only asking / setting the whole joint set of the current Configuration is + * supported. If in the future the support of operating on a subset will be implemented, + * IPositionControl2 and IVelocityControl2 should be implemented. For the time being, + * a possible workaround for this situation is creating a new configuration block + * containing only the reduced set in a deeper-hierarchy Simulink's subsystem. + * @see RobotInterface::getDevice + */ +struct wbt::YarpInterfaces +{ + yarp::dev::IControlMode2* iControlMode2; + yarp::dev::IPositionControl* iPositionControl; + yarp::dev::IPositionDirect* iPositionDirect; + yarp::dev::IVelocityControl* iVelocityControl; + yarp::dev::ITorqueControl* iTorqueControl; + yarp::dev::IPWMControl* iPWMControl; + yarp::dev::ICurrentControl* iCurrentControl; + yarp::dev::IEncoders* iEncoders; + yarp::dev::IControlLimits2* iControlLimits2; + yarp::dev::IPidControl* iPidControl; + + YarpInterfaces() + : iControlMode2(nullptr) + , iPositionControl(nullptr) + , iPositionDirect(nullptr) + , iVelocityControl(nullptr) + , iTorqueControl(nullptr) + , iPWMControl(nullptr) + , iCurrentControl(nullptr) + , iEncoders(nullptr) + , iControlLimits2(nullptr) + , iPidControl(nullptr) + {} +}; + +// TODO o pensare come evitare di avere due conf con es position e torque nei setref con lo stesso robot. +// e' un casino -> aggiungere max un warning o solo documentazione. +// Solo un setReference attivo per configuration.Se ci sono due setReference con lo stesso +// set di giunti attivi contemporaneamente. Oppure due stessi blocchi setReference con lo stesso blocco config. + +/** + * \class wbt::RobotInterface RobotInterface.h + * + * This class holds the configuration used by one or more blocks, and all the objects to operate + * with the specified robot (real or model). + * + * @see wbt::Configuration + * @see wbt::YarpInterfaces + * @see iDynTree::KinDynComputations + */ +class wbt::RobotInterface +{ +private: + std::unique_ptr m_robotDevice; + std::shared_ptr m_kinDynComp; + wbt::YarpInterfaces m_yarpInterfaces; + + // Maps used to store infos about yarp's and idyntree's internal joint indexing + std::shared_ptr m_jointsMapIndex; + std::shared_ptr m_jointsMapString; + std::shared_ptr m_controlledJointsMapCB; + std::shared_ptr m_controlBoardIdxLimit; + + // Configuration from Simulink Block's parameters + const wbt::Configuration m_config; + + // Counters for resource allocation / deallocation + unsigned m_robotDeviceCounter; + + // INITIALIZATION HELPERS + // ====================== + + /** + * Initialize the iDynTree::KinDynComputations with the information contained + * in wbt::Configuration. It finds the urdf file and stores the object to operate on it. + * If the joint list contained in RobotInterface::m_config is not complete, it loads a + * reduced model of the robot + * + * @return True if success + */ + bool initializeModel(); + + /** + * Configure a RemoteControlBoardRemapper device in order to allow + * interfacing the toolbox with the robot (real or in Gazebo). + * + * @return True if success + */ + bool initializeRemoteControlBoardRemapper(); + + // OTHER PRIVATE METHODS + // ===================== + + /** + * Gather a device from the RemoteControlBoardRemapper as a \c weak_ptr + * + * @param device The RemoteControlBoardRemapper device + * @return The dynamic(ally)_cast device + * @tparam T The type of the retured device + */ + template + T* getInterfaceFromTemplate(T*& device); + + /** + * Creates the map between joints (specified as either names or idyntree indices) and + * their YARP representation, which consist in a pair: Control Board index and joint index inside + * the its Control Board. + * + * @see getJointsMapString + * @see getJointsMapIndex + * + * @return True if the map has been created successfully + */ + bool mapDoFs(); + + /** + * Create a RemoteControlBoard object for a given remoteName + * + * @see mapDoFs + * + * @param remoteName [in] Name of the remote from which the remote control board is be initialized + * @param controlBoard [out] Smart pointer to the allocated remote control board + * @return True if success + */ + bool getSingleControlBoard(const std::string& remoteName, std::unique_ptr& controlBoard); +public: + + // CONSTRUCTOR / DESTRUCTOR + // ======================== + + RobotInterface() = delete; + RobotInterface(const wbt::Configuration& c); + ~RobotInterface(); + + // GET METHODS + // =========== + + /** + * Get the current configuration + * + * @return A reference of the Configuration object containing the current configuraton + */ + const wbt::Configuration& getConfiguration() const; + + /** + * Get the map between model joint names and the YARP representation (Control Board and + * joint index) + * + * @return The joint map + */ + const std::shared_ptr getJointsMapString(); + + /** + * Get the map between model joint indices and the YARP representation (Control Board and + * joint index) + * + * @return The joint map + */ + const std::shared_ptr getJointsMapIndex(); + + /** + * Get the map between model joint names and the index representing their relative ordering + * inside the controlledJoints vector relative to their ControlBoard. + * + * @note For example, if the joints are + * \verbatim j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3 \endverbatim, the map links them to + * \verbatim 0 1 2 0 1 0 \end + * + * @return The joint map + */ + const std::shared_ptr getControlledJointsMapCB(); + + /** + * Get the map between the ControlBoard index inside the RemoteControlBoardRemapper + * and the number of the controlled joints which belongs to it. + * + * @note For example, if the joints are + * \verbatim j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3 \endverbatim, the generated map is + * \verbatim {{0,3}{1,2}{2,1}} \endverbatim + * Note that the map key is 0-indexed. + * + * @return The control board limit map + */ + const std::shared_ptr getControlBoardIdxLimit(); + + /** + * Get the object to operate on the configured model + * + * @return A \c shared_ptr to the KinDynComputations object + */ + const std::shared_ptr getKinDynComputations(); + + /** + * Get a \c weak_ptr to an interface from the RemoteControlBoardRemapper + * + * param interface [out] A \c weak_ptr to the interface + * @return True if the \c weak_ptr is valid + * @tparam T The type of interface + */ + template + bool getInterface(T*& interface); + + // LAZY EVALUATION + // =============== + + /** + * Handles the internal counter for using the RemoteControlBoardRemapper + * + * @attention All the blocks which need to use any of the interfaces provided by + * wbt::YarpInterfaces must call this function in their initialize() method. + * @see releaseRemoteControlBoardRemapper + * + * @return True if success + */ + bool retainRemoteControlBoardRemapper(); + + /** + * Handles the internal counter for using the RemoteControlBoardRemapper. After the call + * from the last instance which retained the object, the RemoteControlBoardRemapper and all + * the allocated drivers get destroyed. + * + * @note On the contrary of retainRemoteControlBoardRemapper, this method is already + * called in wbt::~WBBlock + * @see retainRemoteControlBoardRemapper + * + * @return True if success + */ + bool releaseRemoteControlBoardRemapper(); +}; + +#endif /* end of include guard: WBT_ROBOTINTERFACE_H */ diff --git a/toolbox/include/base/Signal.h b/toolbox/include/base/Signal.h index 97f567669..38f827a8b 100644 --- a/toolbox/include/base/Signal.h +++ b/toolbox/include/base/Signal.h @@ -2,47 +2,175 @@ #define WBT_SIGNAL_H #include "BlockInformation.h" +#include +#include namespace wbt { class Signal; + enum SignalDataFormat { + NONCONTIGUOUS = 0, + CONTIGUOUS = 1, + CONTIGUOUS_ZEROCOPY = 2 + }; } -class wbt::Signal { - PortDataType portType; - bool isContiguous; - bool isConstPort; +class wbt::Signal +{ +private: + int m_width; + const bool m_isConst; + const PortDataType m_portDataType; + const SignalDataFormat m_dataFormat; - void** nonContiguousData; - void* contiguousData; + void* m_bufferPtr; + + void deleteBuffer(); + void allocateBuffer(const void* const bufferInput, void*& bufferOutput, unsigned length); + + template T* getCastBuffer() const; public: - Signal(); + // Ctor and Dtor + Signal(const SignalDataFormat& dataFormat = CONTIGUOUS_ZEROCOPY, + const PortDataType& dataType = PortDataTypeDouble, + const bool& isConst = true); + ~Signal(); + // Copy + Signal(const Signal& signal); + Signal& operator=(const Signal& signal) = delete; + // Move + Signal(Signal&& signal); + Signal& operator=(Signal&& signal) = delete; - void initSignalType(wbt::PortDataType type, bool constPort); + bool initializeBufferFromContiguous(const void* buffer); + bool initializeBufferFromContiguousZeroCopy(const void* buffer); + bool initializeBufferFromNonContiguous(const void* const* bufferPtrs); + bool isConst() const; + unsigned getWidth() const; + PortDataType getPortDataType() const; + SignalDataFormat getDataFormat() const; + template T* getBuffer() const; + template T get(const unsigned& i) const; - void setContiguousBuffer(void* buffer); - void setContiguousBuffer(const void* buffer); - void setNonContiguousBuffer(void** buffer); - void setNonContiguousBuffer(const void* const * buffer); + void setWidth(const unsigned& width); + bool set(const unsigned& index, const double& data); + template bool setBuffer(const T* data, const unsigned& length); +}; - const Data get(unsigned index) const; - void* getContiguousBuffer(); - - //the missing are cast - void set(unsigned index, double data); - void setBuffer(const double *data, const unsigned length, unsigned startIndex = 0); +template +T* wbt::Signal::getBuffer() const +{ + // Check the returned matches the same type of the portType. + // If this is not met, appliying pointer arithmetics on the returned + // pointer would show unknown behaviour. + switch(m_portDataType) { + case wbt::PortDataTypeDouble: + if (typeid(T).hash_code() != typeid(double).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeSingle: + if (typeid(T).hash_code() != typeid(float).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeInt8: + if (typeid(T).hash_code() != typeid(int8_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeUInt8: + if (typeid(T).hash_code() != typeid(uint8_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeInt16: + if (typeid(T).hash_code() != typeid(int16_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeUInt16: + if (typeid(T).hash_code() != typeid(uint16_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeInt32: + if (typeid(T).hash_code() != typeid(int32_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeUInt32: + if (typeid(T).hash_code() != typeid(uint32_t).hash_code()) { + return nullptr; + } + break; + case wbt::PortDataTypeBoolean: + if (typeid(T).hash_code() != typeid(bool).hash_code()) { + return nullptr; + } + break; + default: + return nullptr; + break; + } - void set(unsigned index, int32_t data); - void setBuffer(const int32_t *data, const unsigned length, unsigned startIndex = 0); + // Return the correct pointer + return static_cast(m_bufferPtr); +} - void set(unsigned index, uint32_t data); - void setBuffer(const uint32_t *data, const unsigned length, unsigned startIndex = 0); +template +bool wbt::Signal::setBuffer(const T* data, const unsigned& length) +{ + // Non contiguous signals follow the Simulink convention of being read-only + if (m_dataFormat == NONCONTIGUOUS || m_isConst) { + return false; + } - void set(unsigned index, bool data); - void setBuffer(const bool *data, const unsigned length, unsigned startIndex = 0); + if (m_dataFormat == CONTIGUOUS_ZEROCOPY && length > m_width) { + return false; + } -}; + if (typeid(getBuffer()).hash_code() != typeid(T*).hash_code()) { + return false; + } + switch (m_dataFormat) { + case CONTIGUOUS: + // Delete the current array + if (m_bufferPtr) { + delete getBuffer(); + m_bufferPtr = nullptr; + m_width = 0; + } + // Allocate a new empty array + m_bufferPtr = static_cast(new T[length]); + m_width = length; + // Fill it with new data + std::copy(data, data + length, getBuffer()); + break; + case CONTIGUOUS_ZEROCOPY: + // Reset current data + std::fill(getBuffer(), getBuffer() + m_width, 0); + // Copy new data + std::copy(data, data + length, getBuffer()); + // Update the width + m_width = length; + break; + case NONCONTIGUOUS: + return false; + } + + return true; +} + +template +T wbt::Signal::get(const unsigned& i) const +{ + T* buffer = getBuffer(); + assert(buffer); + + return buffer[i]; +} #endif /* end of include guard: WBT_SIGNAL_H */ diff --git a/toolbox/include/base/SimulinkBlockInformation.h b/toolbox/include/base/SimulinkBlockInformation.h index 40b3c36c6..dd79f9b31 100644 --- a/toolbox/include/base/SimulinkBlockInformation.h +++ b/toolbox/include/base/SimulinkBlockInformation.h @@ -2,43 +2,59 @@ #define WBT_SIMULINKBLOCKINFORMATION_H #include "BlockInformation.h" - #include "simstruc.h" +#include "AnyType.h" namespace wbt { class SimulinkBlockInformation; class Signal; } -class wbt::SimulinkBlockInformation : public wbt::BlockInformation { - SimStruct *simstruct; +class wbt::SimulinkBlockInformation : public wbt::BlockInformation +{ +private: + SimStruct* simstruct; + + PortDataType mapSimulinkToPortType(const DTypeId& typeId) const; + DTypeId mapPortTypeToSimulink(const PortDataType& dataType) const; public: - SimulinkBlockInformation(SimStruct *simstruct); - - virtual ~SimulinkBlockInformation(); - - virtual bool optionFromKey(const std::string& key, Data &option) const; - - //Parameters methods - virtual bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter); - virtual Data getScalarParameterAtIndex(unsigned parameterIndex); - - //Port information methods - virtual bool setNumberOfInputPorts(unsigned numberOfPorts); - virtual bool setNumberOfOuputPorts(unsigned numberOfPorts); - virtual bool setInputPortVectorSize(unsigned portNumber, int portSize); - virtual bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns); - virtual bool setOutputPortVectorSize(unsigned portNumber, int portSize); - virtual bool setOutputPortMatrixSize(unsigned portNumber, int rows, int columns); - virtual bool setInputPortType(unsigned portNumber, PortDataType portType); - virtual bool setOutputPortType(unsigned portNumber, PortDataType portType); - - //Port data - virtual unsigned getInputPortWidth(unsigned portNumber); - virtual unsigned getOutputPortWidth(unsigned portNumber); - virtual wbt::Signal getInputPortSignal(unsigned portNumber); - virtual wbt::Signal getOutputPortSignal(unsigned portNumber); + SimulinkBlockInformation(SimStruct* simstruct); + ~SimulinkBlockInformation() override = default; + + // BLOCK OPTIONS METHODS + // ===================== + + bool optionFromKey(const std::string& key, double& option) const override; + + // PARAMETERS METHODS + // ================== + + bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const override; + bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const override; + bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const override; + bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const override; + bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const override; + + // PORT INFORMATION SETTERS + // ======================== + + bool setNumberOfInputPorts(unsigned numberOfPorts) override; + bool setNumberOfOutputPorts(unsigned numberOfPorts) override; + bool setInputPortVectorSize(unsigned portNumber, int portSize) override; + bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns) override; + bool setOutputPortVectorSize(unsigned portNumber, int portSize) override; + bool setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) override; + bool setInputPortType(unsigned portNumber, PortDataType portType) override; + bool setOutputPortType(unsigned portNumber, PortDataType portType) override; + + // PORT INFORMATION GETTERS + // ======================== + + unsigned getInputPortWidth(unsigned portNumber) const override; + unsigned getOutputPortWidth(unsigned portNumber) const override; + wbt::Signal getInputPortSignal(unsigned portNumber, int portWidth = DYNAMICALLY_SIZED) const override; + wbt::Signal getOutputPortSignal(unsigned portNumber, int portWidth = DYNAMICALLY_SIZED) const override; }; #endif /* end of include guard: WBT_SIMULINKBLOCKINFORMATION_H */ diff --git a/toolbox/include/base/ToolboxSingleton.h b/toolbox/include/base/ToolboxSingleton.h new file mode 100644 index 000000000..78dcf47e2 --- /dev/null +++ b/toolbox/include/base/ToolboxSingleton.h @@ -0,0 +1,143 @@ +#ifndef WBT_ToolboxSingleton_H +#define WBT_ToolboxSingleton_H + +#include +#include +#include +#include +#include "Configuration.h" + +namespace wbt { + class ToolboxSingleton; + class RobotInterface; +} + +namespace iDynTree { + class KinDynComputations; +} + +// TODO: check class doxygen macro +/** + * \class ToolboxSingleton ToolboxSingleton.h + * + * This class handles and contains configuration and devices used by all the blocks + * allocated by a running model. It is possible obtaining a singleton of this class + * by calling WBToolbox::sharedInstance. + * + * @see wbt::RobotInterface + * @see wbt::Configuration + * @see wbt::yarpDevices + * + */ +class wbt::ToolboxSingleton { // TODO: wbt::ToolboxSingleton +private: + /// Object that stores all the configurations labelled by the name of the Simulink Block's + /// name. + /// @see wbt::RobotInterface + std::unordered_map> m_interfaces; + +public: + + // CONSTRUCTOR / DESTRUCTOR + // ======================== + + ToolboxSingleton(); + ~ToolboxSingleton(); + + ToolboxSingleton(const ToolboxSingleton&) = delete; + ToolboxSingleton& operator=(const ToolboxSingleton&) = delete; + + // UTILITIES + // ========= + + /** + * Check if a Configuration labelled by confKey exists and is a + * valid object. + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return True if the configuration is valid + */ + bool isKeyValid(const std::string& confKey); + + /** + * Returns the DoFs associated with the configuration labelled by confKey + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return The number of degrees of freedom. It returns -1 when failing. + */ + int numberOfDoFs(const std::string& confKey); + + // GET METHODS + // =========== + + /** + * Returns the singleton instance to this object. + * + * @note The singleton stays in memory until the entire matlab is closed. The WBToolbox + * is designed in such a way that assures to reach a clean state when all blocks + * have been terminated. + * + * @return the singleton instance + */ + static wbt::ToolboxSingleton& sharedInstance(); + + // TODO: check if doxygen ref work + + /** + * Returns the Configuration object labelled by confKey. + * This object is contained into the wbt::RobotInterface object. + * + * @see ToolboxSingleton::getRobotInterface + * @param confKey The key describing the configuration (name of the Simulink block) + * @return A constant reference to the Configuration object + */ + const Configuration& getConfiguration(const std::string& confKey); + + /** + * Returns a \c shared_ptr to the RobotInterface object containing the configuration + * labelled by confKey and all the objects used to gather and set robot data + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return A \c shared_ptr to the RobotInterface of the requested configuration + */ + const std::shared_ptr getRobotInterface(const std::string& confKey); + + /** + * Returns a \c shared_ptr to the KinDynComputations object used to perform calculation + * on the provided model + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @return A \c shared_ptr to the iDynTree::KinDynComputations of the requested + * configuration + */ + const std::shared_ptr getKinDynComputations(const std::string& confKey); + + // TOOLBOXSINGLETON CONFIGURATION + // ============================== + + /*! Saves in the singleton a new configuration \c config. + * + * If the config is valid and hasn't been already stored, it creates a new entry + * in ToolboxSingleton::m_interfaces. If a configuration with matching confKey is found, + * if the Configuration object is the same it does nothing, otherwise it overrides it. + * + * @note Since confKey is the name of the block, the overriding can happen only after + * subsequent compilation in Simulink, when the block's parameters have been + * changed. + * + * @param confKey The key describing the configuration (name of the Simulink block) + * @param config The wbt::Configuration object parsed from Simulink's parameters + * @return Returns \c true if configure is successful, \c false otherwise + * @see ToolboxSingleton::isKeyValid + */ + bool storeConfiguration(const std::string& confKey, const Configuration& config); + + /** + * Delete the wbt::RobotInterface referred by confKey. No-op if it doesn't exist. + * + * @param confKey The key describing the configuration (name of the Simulink block) + */ + void eraseConfiguration(const std::string& confKey); +}; + +#endif /* end of include guard: WBT_ToolboxSingleton_H */ diff --git a/toolbox/include/base/WBBlock.h b/toolbox/include/base/WBBlock.h new file mode 100644 index 000000000..bfd289f96 --- /dev/null +++ b/toolbox/include/base/WBBlock.h @@ -0,0 +1,88 @@ +#ifndef WBT_WBIBLOCK_H +#define WBT_WBIBLOCK_H + +#include "Block.h" +#include +#include +#include +#include +#include +#include +#include + +namespace wbt { + class WBBlock; + class Signal; + class Configuration; + class BlockInformation; + class RobotInterface; +} + +namespace iDynTree { + class MatrixDynSize; +} + +/** + * \struct iDynTreeRobotState WBBlock.h + * + * This struct contains the iDynTree objects used to configure the + * state of iDynTree::KinDynComputations objects. + */ +struct iDynTreeRobotState { + iDynTree::Twist m_baseVelocity; + iDynTree::Vector3 m_gravity; + iDynTree::Transform m_world_T_base; + iDynTree::VectorDynSize m_jointsVelocity; + iDynTree::VectorDynSize m_jointsPosition; + + iDynTreeRobotState() = default; + ~iDynTreeRobotState() = default; + + iDynTreeRobotState(const unsigned& dofs, const std::array& gravity); +}; + +/** + * Basic class for Whole-Body related blocks. + * This class (the whole toolbox in reality) assumes the block represent + * an instantaneous system (i.e. not a dynamic system). + * + * You can create a new block by deriving this class and implementing at least + * the output method. + * + * This block implements the following default behaviours: + * - it ask for 4 parameters (robot name, local (module) name, names of the remote control boards, + * and the list of joints which should all belong to one of the remote control boards) + * - It initializes the yarp network and the whole body interface object + * - During terminate it closes and release the interface object and terminate the yarp network + * + * @note Usually you want to call this class implementations at some point in your + * method overridings, unless you want to completely change the code (but at that point + * you probabily want to derive from Block instead) + */ +class wbt::WBBlock : public wbt::Block +{ +private: + static const unsigned ConfigurationParameterIndex; + static const unsigned ConfBlockNameParameterIndex; + +protected: + std::string confKey; + iDynTreeRobotState robotState; + bool getWBToolboxParameters(Configuration& config, const BlockInformation* blockInfo); + const std::shared_ptr getRobotInterface(); + const Configuration& getConfiguration(); + bool setRobotState(const wbt::Signal* basePose, + const wbt::Signal* jointsPos, + const wbt::Signal* baseVelocity, + const wbt::Signal* jointsVelocity); + +public: + WBBlock() = default; + ~WBBlock() override = default; + unsigned numberOfParameters() override; + bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool initialize(const BlockInformation* blockInfo) override; + bool terminate(const BlockInformation* blockInfo) override; +}; + +#endif /* end of include guard: WBT_WBIBLOCK_H */ diff --git a/toolbox/include/base/WBIBlock.h b/toolbox/include/base/WBIBlock.h deleted file mode 100644 index 33ffe9c36..000000000 --- a/toolbox/include/base/WBIBlock.h +++ /dev/null @@ -1,48 +0,0 @@ -#ifndef WBT_WBIBLOCK_H -#define WBT_WBIBLOCK_H - -#include "Block.h" -#include - -namespace wbt { - class WBIBlock; - class BlockInformation; -} - -/** - * Basic class for WBI related blocks. - * This class (the whole toolbox in reality) assumes the block represent - * an instantaneous system (i.e. not a dynamic system). - * - * You can create a new block by deriving this class and implementing at least - * the output method. - * - * This block implements the following default behaviours: - * - it ask for 4 parameters (robot name, local (module) name, wbi config file and wbi list) - * - It initializes the yarp network and the whole body interface object - * - During terminate it closes and release the interface object and terminate the yarp network - * - * @note Usually you want to call this class implementations at some point in your - * method overridings, unless you want to completely change the code (but at that point - * you probabily want to derive from Block instead) - */ -class wbt::WBIBlock : public wbt::Block { - -protected: - - std::string m_wbiConfigurationFileName; - std::string m_wbiListName; - - bool configureWBIParameters(BlockInformation *blockInfo, wbt::Error *error); - -public: - WBIBlock(); - virtual ~WBIBlock(); - virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); -}; - - -#endif /* end of include guard: WBT_WBIBLOCK_H */ diff --git a/toolbox/include/base/WBIModelBlock.h b/toolbox/include/base/WBIModelBlock.h deleted file mode 100644 index ff054e655..000000000 --- a/toolbox/include/base/WBIModelBlock.h +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef WBT_WBIMODELBLOCK_H -#define WBT_WBIMODELBLOCK_H - -#include "WBIBlock.h" - -namespace wbt { - class WBIModelBlock; - class BlockInformation; -} - -/** - * Basic class for WBI related blocks. - * This class (the whole toolbox in reality) assumes the block represent - * an instantaneous system (i.e. not a dynamic system). - * - * You can create a new block by deriving this class and implementing at least - * the output method. - * - * This block implements the following default behaviours: - * - it ask for 4 parameters (robot name, local (module) name, wbi config file and wbi list) - * - It initializes the yarp network and the whole body interface object - * - During terminate it closes and release the interface object and terminate the yarp network - * - * @Note: Usually you want to call this class implementations at some point in your - * method overridings, unless you want to completely change the code (but at that point - * you probabily want to derive from Block instead) - */ -class wbt::WBIModelBlock : public wbt::WBIBlock { - -public: - virtual ~WBIModelBlock(); - virtual bool initialize(BlockInformation *blockInfo, wbt::Error *error); - virtual bool terminate(BlockInformation *blockInfo, wbt::Error *error); -}; - -#endif /* WBT_WBIMODELBLOCK_H */ diff --git a/toolbox/include/base/WBInterface.h b/toolbox/include/base/WBInterface.h deleted file mode 100644 index bfae00cc9..000000000 --- a/toolbox/include/base/WBInterface.h +++ /dev/null @@ -1,189 +0,0 @@ -#ifndef WBT_WBINTERFACE_H -#define WBT_WBINTERFACE_H - -#include - -namespace wbt { - class WBInterface; - class Error; -} - -namespace wbi { - class wholeBodyInterface; - class iWholeBodyModel; - class IDList; -} -namespace yarp { - namespace os { - class Property; - } -} - -/** - * This class holds a reference counted handle to the whole body interface object. - * You can obtain the singleton reference to this object by calling the sharedInstance method. - * @note at the current state, and because of how the wbi is structured, the model and the interface are - * two different objects, even if this should not lead to any problem - */ -class wbt::WBInterface { - - // Private constructor, destructor, copy constructor and assignemnt operator - WBInterface(); - WBInterface(const WBInterface&); - WBInterface& operator=(const WBInterface&); - ~WBInterface(); - - - wbi::wholeBodyInterface* m_interface; /**< Reference to the interface object */ - wbi::iWholeBodyModel* m_model; /**< Reference to the model object */ - - wbi::IDList *m_robotList; /**< Reference to the joint list object */ - yarp::os::Property *m_configuration; /**< Reference to the configuration used to configure the interface */ - - bool m_initialized; /**< true if the interface has been initialized */ - bool m_modelInitialized; /**< true if the model has been initialized */ - bool m_configured; /**< true if the interface has been configured */ - int m_dofs; /**< dofs modelled by the interface */ - - std::string m_wbiFilePath; - std::string m_wbiListName; - - static unsigned s_referenceCount; /**< number of blocks currently initialized */ - static unsigned s_modelReferenceCount; /**< number of model blocks currently initialized */ - -public: - /** - * Returns the singleton instance to this object. - * - * This is the only way to obtain a reference to an instance of this class - * @return the singleton instance - */ - static wbt::WBInterface& sharedInstance(); - - /** - * Returns the constant pointer to the whole body interface object - * - * @return the whole body interface object - */ - wbi::wholeBodyInterface * const interface(); - - /** - * @return a weak pointer to the model; - */ - wbi::iWholeBodyModel * const model(); - - /** - * @return the current configuration used for the interface and/or model - */ - const yarp::os::Property * const currentConfiguration() const; - - /** - * Returns the degrees of freedom associated with the interface object - * - * @return degrees of freedom of the WBI object - */ - int numberOfDoFs() const; - - /** - * Load the used wbi::IDList from the WBI configuration and the list parameter - * - * @param wbiConfigFile wbi configuration file - * @param list either the name of joint list to be found in the wbi configuration file, - * or directly a list in the format (value1 value2 value3) - * @return true if loading the list was successful, false otherwise. - */ - static bool wbdIDListFromConfigPropAndList(const yarp::os::Property & wbiConfigProp, - const std::string & list, - wbi::IDList & idList); - - /** - * Returns the degrees of freedom for the specified configuration file and the list parameter - * - * @note this method also save the dofs in the internal state. It is thus possible to retrieve it - * by calling numberOfDofs. - * @todo: maybe we should transform this method into const? - * @param wbiConfigFile wbi configuration file - * @param list either the name of joint list to be found in the wbi configuration file, - * or directly a list in the format (Value1 value2 value3) - * - * @return the degrees of freedom of the specified list - */ - int dofsForConfigurationFileAndList(const std::string & wbiConfigFile, const std::string & list); - - /** - * Configure the interface with the specified paramters. - * - * @Note: the interface is configured only once. Subsequent calls are ignored. - * Call clearConfiguration() to clear the configuration. - * @param robotName name of the robot (The model will connect to port with this name as prefix). - * empty the one specified in the configuration file is taken - * @param localName name of the model (ports will be open with this name) - * @param wbiConfigFile name of the wbi configuration file. The file will be looked for by using - * yarp::os::ResourceFinder policies - * @param list either the name of joint list to be found in the wbi configuration file, - * or directly a list in the format (Value1 value2 value3) - * @param [out]error Pointer to the error object to be filled in case of error. - * Check for NULL before using it - * - * @return true if configure is successful, false otherwise. - */ - bool configure(const std::string &robotName, - const std::string & localName, - const std::string & wbiConfigFile, - const std::string & list, - wbt::Error *error); - - /** - * Clear the current configuration, so that configure can be called again. - */ - void clearConfiguration(); - - /** - * Initialize the whole body interface. - * - * This call has effect only the first time. Subsequent calls only increase the reference count - * @Note: Each initialize call should be matched by a subsequent call to terminate. - * @return: true if configure is successful, false otherwise. - */ - bool initialize(); - - /** - * Initialize the model. - * - * This call has effect only the first time. Subsequent calls only increase the reference count - * @Note: Each initialize call should be matched by a subsequent call to terminateModel. - * @return: true if configure is successful, false otherwise. - */ - bool initializeModel(); - - /** - * Release the resources associated with the whole body interface - * - * @Note: resources are released only if the call is made by the last object using the interface - * otherwise the reference count is decreased and no resources are relased. - * @return true if configure is successful, false otherwise. - */ - bool terminate(); - - /** - * Release the resources associated with the model - * - * @Note: resources are released only if the call is made by the last object using the interface - * otherwise the reference count is decreased and no resources are relased. - * @return true if configure is successful, false otherwise. - */ - bool terminateModel(); - - /** - * Returns true if the interface is initialized. False otherwise - * - * @return true if the interface is initialized, false otherwise - */ - bool isInterfaceInitialized() const; - - const wbi::IDList* wbiList() const; - const std::string& wbiFilePath() const; - const std::string& wbiListName() const; -}; - -#endif /* end of include guard: WBT_WBINTERFACE_H */ diff --git a/toolbox/include/base/debug.h b/toolbox/include/base/debug.h new file mode 100644 index 000000000..4fb7c1e7d --- /dev/null +++ b/toolbox/include/base/debug.h @@ -0,0 +1,20 @@ +#ifndef WBT_DEBUG_H +#define WBT_DEBUG_H + +// Debug messages to display in the console from which Matlab was launched +// NDEBUG is defined by CMake in Release and MinSizeRel +// https://gcc.gnu.org/onlinedocs/gcc-4.6.1/gcc/Variadic-Macros.html +#ifndef NDEBUG +#define DEBUG(fmt, ...) \ + fprintf(stderr, "[DEBUG]: %s:%d:%s(): " fmt "\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__) +#define WARNING(fmt, ...) \ + fprintf(stderr, "[WARNING]: %s:%d:%s(): " fmt "\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__) +#define ERROR(fmt, ...) \ + fprintf(stderr, "[ERROR]: %s:%d:%s(): " fmt "\n", __FILE__, __LINE__, __func__, ##__VA_ARGS__) +#else +#define DEBUG(fmt, ...) +#define WARNING(fmt, ...) +#define ERROR(fmt, ...) +#endif + +#endif /* WBT_DEBUG_H */ diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index a61e5e74e..979eb6b93 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -1,8 +1,8 @@ -#include "Error.h" +#include "Log.h" //General Yarp utilities #include "YarpRead.h" #include "YarpWrite.h" -#include "YARPWBIConverter.h" +#include "ModelPartitioner.h" #include "YarpClock.h" //WBI-related stuff #include "MassMatrix.h" @@ -11,18 +11,17 @@ #include "RealTimeSynchronizer.h" #include "SimulatorSynchronizer.h" #include "Jacobian.h" -#include "GetEstimate.h" +#include "GetMeasurement.h" #include "InverseDynamics.h" #include "DotJNu.h" #include "GetLimits.h" #include "CentroidalMomentum.h" +#include "SetLowLevelPID.h" #ifdef WBT_USES_ICUB #include "MinimumJerkTrajectoryGenerator.h" +#include "DiscreteFilter.h" #endif #ifdef WBT_USES_IPOPT -#include "InverseKinematics.h" -#endif -#include "RemoteInverseKinematics.h" -#ifdef WBT_USES_CODYCO_COMMONS -#include "SetLowLevelPID.h" +// #include "InverseKinematics.h" #endif +// #include "RemoteInverseKinematics.h" diff --git a/toolbox/matlab/+WBToolbox/BlockInitialization.m b/toolbox/matlab/+WBToolbox/BlockInitialization.m new file mode 100644 index 000000000..cc335147d --- /dev/null +++ b/toolbox/matlab/+WBToolbox/BlockInitialization.m @@ -0,0 +1,61 @@ +function [configBlock, WBTConfig] = BlockInitialization(currentBlock, currentSystem) +%BLOCKINITIALIZATION Initialize blocks that read a Config block +% This function is used to initialize the mask of block that need to +% gather the WBToolbox configuration from a Config block. + +try + % Get the name of this block from the model's root + %blockAbsoluteName = gcb; + + % Get the top level name + %blockNameSplit = strsplit(blockAbsoluteName,'/'); + blockNameSplit = strsplit(currentBlock,'/'); + topLevel = blockNameSplit{1}; + + % Get all the blocks from the top level of the system + blocks_system = find_system(topLevel,'LookUnderMasks','on','FollowLinks','on'); + + % Get the name of the block's subsystem + %[blockScopeName,~] = fileparts(blockAbsoluteName); + %blockScopeName = gcs; + blockScopeName = currentSystem; +catch + error('[%s::Initialization] Failed to process model''s tree', char(currentBlock)) +end + +% Look a config block from the block's scope going up in the tree +analyzedScope = blockScopeName; +configBlock = char; +while ~isempty(analyzedScope) + rule = strcat(strrep(analyzedScope,'/','\/'),'\/\w+\/ImConfig'); + idx = cellfun(@(x) ~isequal(regexp(x,rule),[]), blocks_system); + if (sum(idx) > 1) + error('[%s::Initialization] Found more than one configuration', char(currentBlock)); + elseif (sum(idx) == 1) + configBlock = blocks_system{idx}; + configBlock = fileparts(configBlock); + break; + end + analyzedScope = fileparts(analyzedScope); +end + +if isempty(configBlock) + error('[%s::Initialization] No configuration found', char(currentBlock)); +end + +configSource = get_param(configBlock,'ConfigSource'); + +if strcmp(configSource,'Mask') + % Read the mask + WBTConfig = WBToolbox.Mask2WBToolboxConfig(configBlock); +elseif strcmp(configSource,'Workspace') + % Get the object name and populate the mask + configObjectName = get_param(configBlock,'ConfigObject'); + WBToolbox.WBToolboxConfig2Mask(configBlock, configObjectName); + % Read the mask + WBTConfig = WBToolbox.Mask2WBToolboxConfig(configBlock); +else + error('[%s::Initialization] Read ConfigSource from Config block not recognized', char(currentBlock)); +end + +end diff --git a/toolbox/matlab/+WBToolbox/Mask2WBToolboxConfig.m b/toolbox/matlab/+WBToolbox/Mask2WBToolboxConfig.m new file mode 100644 index 000000000..b9850d12c --- /dev/null +++ b/toolbox/matlab/+WBToolbox/Mask2WBToolboxConfig.m @@ -0,0 +1,39 @@ +function [ WBTConfig ] = Mask2WBToolboxConfig(configBlock) +%MASK2WBTOOLBOXCONFIG Summary of this function goes here +% Detailed explanation goes here + +defaultString = '''NonValid'''; + +if (... + strcmp(char(get_param(configBlock,'RobotName')),defaultString) && ... + strcmp(char(get_param(configBlock,'UrdfFile')),defaultString) && ... + strcmp(char(get_param(configBlock,'LocalName')),defaultString) && ... + strcmp(char(get_param(configBlock,'GravityVector')),defaultString) && ... + strcmp(char(get_param(configBlock,'ControlledJoints')),defaultString) && ... + strcmp(char(get_param(configBlock,'ControlBoardsNames')),defaultString) ... + ) + error('[Mask2WBToolboxConfig] The provided config is NonValid.'); +end + +WBTConfig = WBToolbox.WBToolboxConfig; + +WBTConfig.RobotName = stripApices(char(get_param(configBlock,'RobotName'))); +WBTConfig.UrdfFile = stripApices(char(get_param(configBlock,'UrdfFile'))); +WBTConfig.LocalName = stripApices(char(get_param(configBlock,'LocalName'))); + +ControlledJointsChar = stripApices(char(get_param(configBlock,'ControlledJoints'))); +WBTConfig.ControlledJoints = evalin('base',ControlledJointsChar); + +ControlBoardsNamesChar = stripApices(char(get_param(configBlock,'ControlBoardsNames'))); +WBTConfig.ControlBoardsNames = evalin('base',ControlBoardsNamesChar); + +GravityVectorChar = stripApices(char(get_param(configBlock,'GravityVector'))); +WBTConfig.GravityVector = evalin('base',GravityVectorChar); + +end + +function str = stripApices(str) + if (strcmp(str(1),'''') && strcmp(str(end),'''')) + str = str(2:end-1); + end +end diff --git a/toolbox/matlab/+WBToolbox/PID.m b/toolbox/matlab/+WBToolbox/PID.m new file mode 100644 index 000000000..33badfea0 --- /dev/null +++ b/toolbox/matlab/+WBToolbox/PID.m @@ -0,0 +1,50 @@ +classdef PID < handle + %PID Class to store pid parameters for a single joint. + % PID('jointName') creates an object for the joint 'jointName' with + % all pid gains set to 0. + % + % PID('jointName', P, I, D) creates an object for the joint 'jointName' + % initializing the gains respectively to P, I, D. + % + % PID Properties: + % P - Proportional gain + % I - Integral gain + % D - Derivative gain + % joint - Reference joint + % + % See also: WBTOOLBOX.WBTPIDCONFIG + + properties + P (1,1) double = 0 + I (1,1) double = 0 + D (1,1) double = 0 + joint (1,:) char + end + + methods + % function obj = PID(jointName) + % obj.joint = jointName; + % end + + function obj = PID(jointName, P, I, D) + if (nargin == 4) + obj.joint = jointName; + obj.P = P; + obj.I = I; + obj.D = D; + elseif (nargin == 1) + obj.joint = jointName; + else + error(... + [... + 'Wrong number of arguments: ', ... + 'You can either specify 1 argument (joint) ', ... + 'or 4 arguments (joint, P, I, D).',... + ]... + ); + end + end + + end +end + diff --git a/toolbox/matlab/+WBToolbox/WBTPIDConfig.m b/toolbox/matlab/+WBToolbox/WBTPIDConfig.m new file mode 100644 index 000000000..51999e301 --- /dev/null +++ b/toolbox/matlab/+WBToolbox/WBTPIDConfig.m @@ -0,0 +1,75 @@ +classdef WBTPIDConfig < handle + % WBTPIDCONFIG Class to store a set of PID config for many joints. + % WBTPIDCONFIG(WBToolbox.PID('jointName', P, I, D)) adds a new PID + % for joint 'jointName'. + % + % WBTPIDCONFIG Properties: + % P - Proportional gains + % I - Integral gains + % D - Derivative gains + % jointList - List of joints + % + % WBTPIDCONFIG Methods: + % addPID - Add a new joint PID + % removePID - Remove a stored joint PID + % + % See also: WBTOOLBOX.PID + + % Read-Only properties + properties (SetAccess = private) + P (1,:) double + I (1,:) double + D (1,:) double + jointList (1,:) cell + end + + methods + function addPID(obj, pid) + % Add + if ~isa(pid,'WBToolbox.PID') + error('The argument must be a WBToolbox.PID object.') + end + + matches = find(strcmp(obj.jointList, pid.joint), 1); + if ~isempty(matches) + error('The PID''s joint has been already processed.') + end + + obj.jointList{1, end+1} = pid.joint; + obj.P(1, end+1) = pid.P; + obj.I(1, end+1) = pid.I; + obj.D(1, end+1) = pid.D; + end + + function removePID(obj, jointName) + if ~ischar(jointName) + error('The argument must be a char containing the joint name.') + end + + idx = find(strcmp(obj.jointList, jointName), 1); + + if isempty(idx) + error('The specified joint has no configuration stored') + end + + obj.jointList(:,idx) = []; + obj.P(:,idx) = []; + obj.I(:,idx) = []; + obj.D(:,idx) = []; + end + + function value = getSimulinkParameters(obj) + if isempty(obj.P) || isempty(obj.I) || isempty(obj.D) || ... + isempty(obj.jointList) + error('Trying to get parameters from an empty object') + end + value = struct(); + value.P = obj.P; + value.I = obj.I; + value.D = obj.D; + value.jointList = obj.jointList; + end + end + +end + diff --git a/toolbox/matlab/+WBToolbox/WBToolboxConfig.m b/toolbox/matlab/+WBToolbox/WBToolboxConfig.m new file mode 100644 index 000000000..20374e498 --- /dev/null +++ b/toolbox/matlab/+WBToolbox/WBToolboxConfig.m @@ -0,0 +1,143 @@ +classdef WBToolboxConfig < matlab.mixin.Copyable + %WBTOOLBOXCONFIG Summary of this class goes here + % Detailed explanation goes here + + % PROPERTIES + % ========== + + properties + % Robot state + RobotName (1,:) char + UrdfFile (1,:) char + ControlledJoints (1,:) cell + ControlBoardsNames (1,:) cell + LocalName (1,:) char + % Other Variables + GravityVector (1,3) double = [0, 0, -9.81] + end + + properties (Dependent) + ValidConfiguration (1,1) logical + end + + properties (Hidden, Dependent) + SimulinkParameters (1,1) struct + end + + % METHODS + % ======= + + methods + + % SET METHODS + % ----------- + + % Dependent properties + % -------------------- + + function set.ValidConfiguration(~,~) + error(['ValidConfiguration is set to 1 automatically ', ... + 'when the configuration is valid.']); + end + + function set.SimulinkParameters(~,~) + error(['SimulinkParameters is created automatically ', ... + 'when the configuration is valid.']); + end + + % Other properties + % ---------------- + + function set.ControlledJoints(obj, value) + if (~iscellstr(value)) + error(['ControlledJoints must be a cell array of ', ... + 'charachter vectors']); + end + obj.ControlledJoints = value; + end + + function set.ControlBoardsNames(obj, value) + if (~iscellstr(value)) + error(['ControlBoardsNames must be a cell array of ', ... + 'charachter vectors']); + end + obj.ControlBoardsNames = value; + end + + % GET METHODS + % ----------- + + % Dependent properties + % -------------------- + + function value = get.ValidConfiguration(obj) + % Check if all the properties have been set + value = ... + ~isempty(obj.RobotName) && ... + ~isempty(obj.UrdfFile) && ... + ~isempty(obj.ControlledJoints) && ... + ~isempty(obj.ControlBoardsNames) && ... + ~isempty(obj.LocalName) && ... + ~isequal(obj.GravityVector,[0, 0, 0]); + end + + function value = get.SimulinkParameters(obj) + % Create and populate the struct + value = struct(); + value.RobotName = obj.RobotName; + value.UrdfFile = obj.UrdfFile; + value.LocalName = obj.LocalName; + value.ControlledJoints = obj.ControlledJoints; + value.ControlBoardsNames = obj.ControlBoardsNames; + value.GravityVector = obj.GravityVector; + end + + % Other properties + % ---------------- + + % OTHER METHODS + % ============= + + function value = getSimulinkParameters(obj) + if (obj.ValidConfiguration) + value = obj.SimulinkParameters; + else + error('The configuration is not complete'); + end + end + end + + methods(Static) + + function cellArraySerialized = serializeCellArray1D(cellArray) + [m,n] = size(cellArray); + if (m>1 && n>1) + error('The input cell must be 1D'); + end + cellArraySerialized = '{'; + for i=1:length(cellArray) + cellArraySerialized = strcat(cellArraySerialized,"'",cellArray{i},"'"); + if i ~= length(cellArray) + cellArraySerialized = strcat(cellArraySerialized,','); + end + end + cellArraySerialized = strcat(cellArraySerialized,'}'); + end + + function vectorSerialized = serializeVector1D(vector) + [m,n] = size(vector); + if (m>1 && n>1) + error('The input vector must be 1D'); + end + vectorSerialized = '['; + for i=1:length(vector) + vectorSerialized = strcat(vectorSerialized,num2str(vector(i))); + if i ~= length(vector) + vectorSerialized = strcat(vectorSerialized,','); + end + end + vectorSerialized = strcat(vectorSerialized,']'); + end + + end +end diff --git a/toolbox/matlab/+WBToolbox/WBToolboxConfig2Mask.m b/toolbox/matlab/+WBToolbox/WBToolboxConfig2Mask.m new file mode 100644 index 000000000..1e7e3b227 --- /dev/null +++ b/toolbox/matlab/+WBToolbox/WBToolboxConfig2Mask.m @@ -0,0 +1,90 @@ +function WBToolboxConfig2Mask(configBlock, WBTConfigObjectName) +%WBTOOLBOXCONFIG2MASK Summary of this function goes here +% Detailed explanation goes here + +% Assign names to labels +item_ConfigSource = 1; +item_ConfigObject = 2; +item_RobotName = 3; +item_UrdfFile = 4; +item_ControlledJoints = 5; +item_ControlBoardsNames = 6; +item_LocalName = 7; +item_GravityVector = 8; + +% Get the mask of the Config Block +try + mask = Simulink.Mask.get(configBlock); +catch + error('[WBToolboxConfig2Mask] Impossible to gather Config Block: %s', configBlock); +end + + +% Remove apices if present +if (strcmp(WBTConfigObjectName(1),'''') && strcmp(WBTConfigObjectName(end),'''')) + WBTConfigObjectName = WBTConfigObjectName(2:end-1); +end + +% Try to get the object from the workspace +try + WBTConfigHandle = evalin('base', WBTConfigObjectName); % Get the handle + WBTConfigObject = copy(WBTConfigHandle); % Shallow copy the handle +catch + error('[WBToolboxConfig2Mask] Failed to get %s object from the workspace', WBTConfigObjectName); +end + +try + if (isa(WBTConfigObject,'WBToolbox.WBToolboxConfig') && WBTConfigObject.ValidConfiguration) + SetParamFromReadOnly(... + configBlock,'RobotName',... + strcat('''',WBTConfigObject.RobotName,''''),... + mask.Parameters(item_RobotName)); + SetParamFromReadOnly(... + configBlock,'UrdfFile',... + strcat('''',WBTConfigObject.UrdfFile,''''),mask.Parameters(item_UrdfFile)); + SetParamFromReadOnly(... + configBlock,'ControlledJoints',... + WBTConfigObject.serializeCellArray1D(WBTConfigObject.ControlledJoints),mask.Parameters(item_ControlledJoints)); + SetParamFromReadOnly(... + configBlock,'ControlBoardsNames',... + WBTConfigObject.serializeCellArray1D(WBTConfigObject.ControlBoardsNames),mask.Parameters(item_ControlBoardsNames)); + SetParamFromReadOnly(... + configBlock,'GravityVector',... + WBTConfigObject.serializeVector1D(WBTConfigObject.GravityVector),mask.Parameters(item_GravityVector)) + SetParamFromReadOnly(... + configBlock,'LocalName',... + strcat('''',WBTConfigObject.LocalName,''''),mask.Parameters(item_LocalName)); + else + error('[WBToolboxConfig2Mask] Failed to get the %s object', WBTConfigObjectName); + end +catch + SetParamFromReadOnly(... + configBlock,'RobotName',... + '''NonValid''',mask.Parameters(item_RobotName)); + SetParamFromReadOnly(... + configBlock,'UrdfFile',... + '''NonValid''',mask.Parameters(item_UrdfFile)); + SetParamFromReadOnly(... + configBlock,'ControlledJoints',... + '''NonValid''',mask.Parameters(item_ControlledJoints)); + SetParamFromReadOnly(... + configBlock,'ControlBoardsNames',... + '''NonValid''',mask.Parameters(item_ControlBoardsNames)); + SetParamFromReadOnly(... + configBlock,'LocalName',... + '''NonValid''',mask.Parameters(item_LocalName)); + SetParamFromReadOnly(... + configBlock,'GravityVector',... + '''NonValid''',mask.Parameters(item_GravityVector)); +end + +end + +function SetParamFromReadOnly(block, parameterName, parameterValue, maskParameter) +if ~strcmp(get_param(block,parameterName),parameterValue) + maskParameter.ReadOnly = 'off'; + maskParameter.Enabled = 'on'; + set_param(block,parameterName,parameterValue); + maskParameter.ReadOnly = 'on'; +end +end diff --git a/toolbox/export_library.m b/toolbox/matlab/export_library.m similarity index 56% rename from toolbox/export_library.m rename to toolbox/matlab/export_library.m index 9044fda4d..c452b7fd1 100644 --- a/toolbox/export_library.m +++ b/toolbox/matlab/export_library.m @@ -1,4 +1,4 @@ -addpath(genpath('../images')); +addpath(genpath('../../images')); fprintf('\nWhole Body toolbox exporting library to multiple versions\n'); @@ -7,27 +7,35 @@ quit; end +addpath(genpath('library')); libraryName = 'WBToolboxLibrary_repository'; try + % Load the library open_system(libraryName,'loadonly'); fprintf('\nLibrary loaded\n'); + + % Enable library capabilities if (strcmp(get_param(libraryName, 'EnableLBRepository'), 'off')) set_param(libraryName, 'Lock', 'off'); set_param(libraryName, 'EnableLBRepository', 'on'); set_param(libraryName, 'Lock', 'on'); - end; + end + + % Export the library fprintf('\nExporting for 2014b\n'); - % This does not completely work: images are not saved. - % Instad if saved form the GUI it works.. save_system(libraryName, 'WBToolboxLibrary', 'ExportToVersion', 'R2014B_SLX'); - fprintf('\nExporting for 2012a\n'); - save_system(libraryName, 'WBToolboxLibrary', 'ExportToVersion', 'R2012A_MDL'); + movefile('WBToolboxLibrary.slx', 'library/exported/WBToolboxLibrary.slx'); + % TODO: Check if mdl support is still required + % fprintf('\nExporting for 2012a\n'); + % save_system(libraryName, 'WBToolboxLibrary', 'ExportToVersion', 'R2012A_MDL'); + % movefile('WBToolboxLibrary.mdl', 'library/exported/WBToolboxLibrary.mdl'); + + % Unload the library close_system(libraryName); -catch ex; +catch + error('Failed to export WBToolbox library') end - + fprintf('\nDone\n'); exit(0) - - diff --git a/toolbox/matlab/library/WBToolboxLibrary_repository.mdl b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl new file mode 100644 index 000000000..1083107e5 --- /dev/null +++ b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl @@ -0,0 +1,6078 @@ +Library { + Name "WBToolboxLibrary_repository" + Version 8.9 + SavedCharacterEncoding "US-ASCII" + LogicAnalyzerGraphicalSettings "" + LogicAnalyzerPlugin "on" + LogicAnalyzerSignalOrdering "" + DiagnosticSuppressor "on" + SuppressorTable "22 serialization::archive 11 0 6 0 0 0 11 0" + SLCCPlugin "on" + LibraryType "BlockLibrary" + LockLinksToLibrary on + ScopeRefreshTime 0.035000 + OverrideScopeRefreshTime on + DisableAllScopes off + FPTRunName "Run 1" + MaxMDLFileLineLength 120 + LastSavedArchitecture "glnxa64" + Object { + $PropName "BdWindowsInfo" + $ObjectID 1 + $ClassName "Simulink.BDWindowsInfo" + Object { + $PropName "WindowsInfo" + $ObjectID 2 + $ClassName "Simulink.WindowInfo" + IsActive [1] + Location [2086.0, 89.0, 1595.0, 866.0] + Object { + $PropName "ModelBrowserInfo" + $ObjectID 3 + $ClassName "Simulink.ModelBrowserInfo" + Visible [0] + DockPosition "Left" + Width [50] + Height [50] + Filter [9] + } + Object { + $PropName "ExplorerBarInfo" + $ObjectID 4 + $ClassName "Simulink.ExplorerBarInfo" + Visible [1] + } + Array { + Type "Simulink.EditorInfo" + Dimension 2 + Object { + $ObjectID 5 + IsActive [0] + ViewObjType "SimulinkSubsys" + LoadSaveID "369" + Extents [1557.0, 693.0] + ZoomFactor [2.6923076923076925] + Offset [178.33132569815649, 95.800000000000011] + } + Object { + $ObjectID 6 + IsActive [1] + ViewObjType "SimulinkSubsys" + LoadSaveID "192" + Extents [1557.0, 726.0] + ZoomFactor [2.13] + Offset [-46.444835680751169, 21.953051643192453] + } + PropName "EditorsInfo" + } + Object { + $PropName "DockComponentsInfo" + $ObjectID 7 + $ClassName "Simulink.DockComponentInfo" + Type "GLUE2:PropertyInspector" + ID "Property Inspector" + Visible [0] + CreateCallback "" + UserData "" + Floating [0] + DockPosition "Right" + Width [640] + Height [480] + } + WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAACivwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" + "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" + "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAANQAAAooAAABbAP///wAAAAEAAAAAAAAAAPwCA" + "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" + "QBjAHQAbwByAAAAAAD/////AAAAKwD///8AAAY7AAADFwAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" + "////wEAAAB6/////wAAAAAAAAAA/////wEAAADe/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAEt/////wAAAAAAAAAA/" + "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" + } + } + HideAutomaticNames off + Created "Thu Feb 06 02:21:39 2014" + Creator "jorhabib" + UpdateHistory "UpdateHistoryNever" + ModifiedByFormat "%" + LastModifiedBy "dferigo" + ModifiedDateFormat "%" + LastModifiedDate "Wed Dec 06 10:03:50 2017" + RTWModifiedTimeStamp 434455429 + ModelVersionFormat "1.%" + ConfigurationManager "none" + SampleTimeColors off + SampleTimeAnnotations off + LibraryLinkDisplay "disabled" + WideLines off + ShowLineDimensions off + ShowPortDataTypes off + PortDataTypeDisplayFormat "AliasTypeOnly" + ShowEditTimeErrors on + ShowEditTimeWarnings on + ShowEditTimeAdvisorChecks off + ShowPortUnits off + ShowDesignRanges off + ShowLoopsOnError on + IgnoreBidirectionalLines off + ShowStorageClass off + ShowTestPointIcons on + ShowSignalResolutionIcons on + ShowViewerIcons on + SortedOrder off + VariantCondition off + ExecutionContextIcon off + ShowLinearizationAnnotations on + ShowVisualizeInsertedRTB on + ShowMarkup on + BlockNameDataTip off + BlockParametersDataTip off + BlockDescriptionStringDataTip off + ToolBar on + StatusBar on + BrowserShowLibraryLinks off + FunctionConnectors off + BrowserLookUnderMasks off + SimulationMode "normal" + VisualizeLoggedSignalsWhenLoggingToFile off + PauseTimes "5" + NumberOfSteps 1 + SnapshotBufferSize 10 + SnapshotInterval 10 + NumberOfLastSnapshots 0 + LinearizationMsg "none" + Profile off + ParamWorkspaceSource "MATLABWorkspace" + ExtModeBatchMode off + ExtModeEnableFloating on + ExtModeTrigType "manual" + ExtModeTrigMode "normal" + ExtModeTrigPort "1" + ExtModeTrigElement "any" + ExtModeTrigDuration 1000 + ExtModeTrigDurationFloating "auto" + ExtModeTrigHoldOff 0 + ExtModeTrigDelay 0 + ExtModeTrigDirection "rising" + ExtModeTrigLevel 0 + ExtModeArchiveMode "off" + ExtModeAutoIncOneShot off + ExtModeIncDirWhenArm off + ExtModeAddSuffixToVar off + ExtModeWriteAllDataToWs off + ExtModeArmWhenConnect on + ExtModeSkipDownloadWhenConnect off + ExtModeLogAll on + ExtModeAutoUpdateStatusClock on + ShowModelReferenceBlockVersion off + ShowModelReferenceBlockIO off + OrderedModelArguments on + Array { + Type "Handle" + Dimension 1 + Simulink.ConfigSet { + $ObjectID 8 + Version "1.17.0" + DisabledProps [] + Description "" + Array { + Type "Handle" + Dimension 10 + Simulink.SolverCC { + $ObjectID 9 + Version "1.17.0" + DisabledProps [] + Description "" + StartTime "0.0" + StopTime "10.0" + AbsTol "auto" + FixedStep "auto" + InitialStep "auto" + MaxNumMinSteps "-1" + MaxOrder 5 + ZcThreshold "auto" + ConsecutiveZCsStepRelTol "10*128*eps" + MaxConsecutiveZCs "1000" + ExtrapolationOrder 4 + NumberNewtonIterations 1 + MaxStep "auto" + MinStep "auto" + MaxConsecutiveMinStep "1" + RelTol "1e-3" + EnableMultiTasking on + EnableConcurrentExecution off + ConcurrentTasks off + Solver "ode45" + SolverName "ode45" + SolverJacobianMethodControl "auto" + ShapePreserveControl "DisableAll" + ZeroCrossControl "UseLocalSettings" + ZeroCrossAlgorithm "Nonadaptive" + AlgebraicLoopSolver "TrustRegion" + SolverInfoToggleStatus off + IsAutoAppliedInSIP off + SolverResetMethod "Fast" + PositivePriorityOrder off + AutoInsertRateTranBlk off + SampleTimeConstraint "Unconstrained" + InsertRTBMode "Whenever possible" + SampleTimeProperty [] + } + Simulink.DataIOCC { + $ObjectID 10 + Version "1.17.0" + DisabledProps [] + Description "" + Decimation "1" + ExternalInput "[t, u]" + FinalStateName "xFinal" + InitialState "xInitial" + LimitDataPoints on + MaxDataPoints "1000" + LoadExternalInput off + LoadInitialState off + SaveFinalState off + SaveCompleteFinalSimState off + SaveFormat "Array" + SignalLoggingSaveFormat "ModelDataLogs" + SaveOutput on + SaveState off + SignalLogging on + DSMLogging on + InspectSignalLogs off + VisualizeSimOutput on + StreamToWorkspace off + StreamVariableName "streamout" + SaveTime on + ReturnWorkspaceOutputs off + StateSaveName "xout" + TimeSaveName "tout" + OutputSaveName "yout" + SignalLoggingName "logsout" + DSMLoggingName "dsmout" + OutputOption "RefineOutputTimes" + OutputTimes "[]" + ReturnWorkspaceOutputsName "out" + Refine "1" + LoggingToFile off + DatasetSignalFormat "timeseries" + LoggingFileName "out.mat" + LoggingIntervals "[-inf, inf]" + } + Simulink.OptimizationCC { + $ObjectID 11 + Version "1.17.0" + Array { + Type "Cell" + Dimension 8 + Cell "BooleansAsBitfields" + Cell "PassReuseOutputArgsAs" + Cell "PassReuseOutputArgsThreshold" + Cell "ZeroExternalMemoryAtStartup" + Cell "ZeroInternalMemoryAtStartup" + Cell "OptimizeModelRefInitCode" + Cell "NoFixptDivByZeroProtection" + Cell "UseSpecifiedMinMax" + PropName "DisabledProps" + } + Description "" + BlockReduction on + BooleanDataType on + ConditionallyExecuteInputs on + DefaultParameterBehavior "Tunable" + UseDivisionForNetSlopeComputation "off" + UseFloatMulNetSlope off + DefaultUnderspecifiedDataType "double" + UseSpecifiedMinMax off + InlineInvariantSignals off + OptimizeBlockIOStorage on + BufferReuse on + EnhancedBackFolding off + CachingGlobalReferences off + GlobalBufferReuse on + StrengthReduction off + AdvancedOptControl "" + ExpressionFolding on + BooleansAsBitfields off + BitfieldContainerType "uint_T" + EnableMemcpy on + MemcpyThreshold 64 + PassReuseOutputArgsAs "Structure reference" + PassReuseOutputArgsThreshold 12 + ExpressionDepthLimit 2147483647 + LocalBlockOutputs on + RollThreshold 5 + StateBitsets off + DataBitsets off + ActiveStateOutputEnumStorageType "Native Integer" + ZeroExternalMemoryAtStartup on + ZeroInternalMemoryAtStartup on + InitFltsAndDblsToZero off + NoFixptDivByZeroProtection off + EfficientFloat2IntCast off + EfficientMapNaN2IntZero on + LifeSpan "inf" + MaxStackSize "Inherit from target" + BufferReusableBoundary on + SimCompilerOptimization "off" + AccelVerboseBuild off + OptimizeBlockOrder "off" + OptimizeDataStoreBuffers on + BusAssignmentInplaceUpdate on + } + Simulink.DebuggingCC { + $ObjectID 12 + Version "1.17.0" + Array { + Type "Cell" + Dimension 1 + Cell "UseOnlyExistingSharedCode" + PropName "DisabledProps" + } + Description "" + RTPrefix "error" + ConsistencyChecking "none" + ArrayBoundsChecking "none" + SignalInfNanChecking "none" + SignalRangeChecking "none" + ReadBeforeWriteMsg "UseLocalSettings" + WriteAfterWriteMsg "UseLocalSettings" + WriteAfterReadMsg "UseLocalSettings" + AlgebraicLoopMsg "warning" + ArtificialAlgebraicLoopMsg "warning" + SaveWithDisabledLinksMsg "warning" + SaveWithParameterizedLinksMsg "warning" + CheckSSInitialOutputMsg on + UnderspecifiedInitializationDetection "Classic" + MergeDetectMultiDrivingBlocksExec "none" + CheckExecutionContextPreStartOutputMsg off + CheckExecutionContextRuntimeOutputMsg off + SignalResolutionControl "UseLocalSettings" + BlockPriorityViolationMsg "warning" + MinStepSizeMsg "warning" + TimeAdjustmentMsg "none" + MaxConsecutiveZCsMsg "error" + MaskedZcDiagnostic "warning" + IgnoredZcDiagnostic "warning" + SolverPrmCheckMsg "warning" + InheritedTsInSrcMsg "warning" + MultiTaskDSMMsg "error" + MultiTaskCondExecSysMsg "error" + MultiTaskRateTransMsg "error" + SingleTaskRateTransMsg "none" + TasksWithSamePriorityMsg "warning" + SigSpecEnsureSampleTimeMsg "warning" + CheckMatrixSingularityMsg "none" + IntegerOverflowMsg "warning" + Int32ToFloatConvMsg "warning" + ParameterDowncastMsg "error" + ParameterOverflowMsg "error" + ParameterUnderflowMsg "none" + ParameterPrecisionLossMsg "warning" + ParameterTunabilityLossMsg "warning" + FixptConstUnderflowMsg "none" + FixptConstOverflowMsg "none" + FixptConstPrecisionLossMsg "none" + UnderSpecifiedDataTypeMsg "none" + UnnecessaryDatatypeConvMsg "none" + VectorMatrixConversionMsg "none" + InvalidFcnCallConnMsg "error" + FcnCallInpInsideContextMsg "warning" + SignalLabelMismatchMsg "none" + UnconnectedInputMsg "warning" + UnconnectedOutputMsg "warning" + UnconnectedLineMsg "warning" + UseOnlyExistingSharedCode "error" + SFcnCompatibilityMsg "none" + FrameProcessingCompatibilityMsg "error" + UniqueDataStoreMsg "none" + BusObjectLabelMismatch "warning" + RootOutportRequireBusObject "warning" + AssertControl "UseLocalSettings" + AllowSymbolicDim on + RowMajorDimensionSupport off + ModelReferenceIOMsg "none" + ModelReferenceMultiInstanceNormalModeStructChecksumCheck "error" + ModelReferenceVersionMismatchMessage "none" + ModelReferenceIOMismatchMessage "none" + UnknownTsInhSupMsg "warning" + ModelReferenceDataLoggingMessage "warning" + ModelReferenceSymbolNameMessage "warning" + ModelReferenceExtraNoncontSigs "error" + StateNameClashWarn "warning" + SimStateInterfaceChecksumMismatchMsg "warning" + SimStateOlderReleaseMsg "error" + InitInArrayFormatMsg "warning" + StrictBusMsg "ErrorLevel1" + BusNameAdapt "WarnAndRepair" + NonBusSignalsTreatedAsBus "none" + SymbolicDimMinMaxWarning "warning" + LossOfSymbolicDimsSimulationWarning "warning" + LossOfSymbolicDimsCodeGenerationWarning "error" + SymbolicDimsDataTypeCodeGenerationDiagnostic "error" + BlockIODiagnostic "none" + SFUnusedDataAndEventsDiag "warning" + SFUnexpectedBacktrackingDiag "warning" + SFInvalidInputDataAccessInChartInitDiag "warning" + SFNoUnconditionalDefaultTransitionDiag "warning" + SFTransitionOutsideNaturalParentDiag "warning" + SFUnreachableExecutionPathDiag "warning" + SFUndirectedBroadcastEventsDiag "warning" + SFTransitionActionBeforeConditionDiag "warning" + SFOutputUsedAsStateInMooreChartDiag "error" + SFTemporalDelaySmallerThanSampleTimeDiag "warning" + SFSelfTransitionDiag "warning" + SFExecutionAtInitializationDiag "none" + SFMachineParentedDataDiag "warning" + IntegerSaturationMsg "warning" + AllowedUnitSystems "all" + UnitsInconsistencyMsg "warning" + AllowAutomaticUnitConversions on + UnitDatabase "" + } + Simulink.HardwareCC { + $ObjectID 13 + Version "1.17.0" + DisabledProps [] + Description "" + ProdBitPerChar 8 + ProdBitPerShort 16 + ProdBitPerInt 32 + ProdBitPerLong 32 + ProdBitPerLongLong 64 + ProdBitPerFloat 32 + ProdBitPerDouble 64 + ProdBitPerPointer 32 + ProdBitPerSizeT 32 + ProdBitPerPtrDiffT 32 + ProdLargestAtomicInteger "Char" + ProdLargestAtomicFloat "None" + ProdIntDivRoundTo "Undefined" + ProdEndianess "Unspecified" + ProdWordSize 32 + ProdShiftRightIntArith on + ProdLongLongMode off + ProdHWDeviceType "32-bit Generic" + TargetBitPerChar 8 + TargetBitPerShort 16 + TargetBitPerInt 32 + TargetBitPerLong 32 + TargetBitPerLongLong 64 + TargetBitPerFloat 32 + TargetBitPerDouble 64 + TargetBitPerPointer 32 + TargetBitPerSizeT 32 + TargetBitPerPtrDiffT 32 + TargetLargestAtomicInteger "Char" + TargetLargestAtomicFloat "None" + TargetShiftRightIntArith on + TargetLongLongMode off + TargetIntDivRoundTo "Undefined" + TargetEndianess "Unspecified" + TargetWordSize 32 + TargetPreprocMaxBitsSint 32 + TargetPreprocMaxBitsUint 32 + TargetHWDeviceType "Specified" + TargetUnknown off + ProdEqTarget on + UseEmbeddedCoderFeatures on + UseSimulinkCoderFeatures on + } + Simulink.ModelReferenceCC { + $ObjectID 14 + Version "1.17.0" + DisabledProps [] + Description "" + UpdateModelReferenceTargets "IfOutOfDateOrStructuralChange" + EnableRefExpFcnMdlSchedulingChecks on + CheckModelReferenceTargetMessage "error" + EnableParallelModelReferenceBuilds off + ParallelModelReferenceErrorOnInvalidPool on + ParallelModelReferenceMATLABWorkerInit "None" + ModelReferenceNumInstancesAllowed "Multi" + PropagateVarSize "Infer from blocks in model" + ModelDependencies "" + ModelReferencePassRootInputsByReference on + ModelReferenceMinAlgLoopOccurrences off + PropagateSignalLabelsOutOfModel off + SupportModelReferenceSimTargetCustomCode off + } + Simulink.SFSimCC { + $ObjectID 15 + Version "1.17.0" + DisabledProps [] + Description "" + SimCustomSourceCode "" + SimCustomHeaderCode "" + SimCustomInitializer "" + SimCustomTerminator "" + SimReservedNameArray [] + SimUserSources "" + SimUserIncludeDirs "" + SimUserLibraries "" + SimUserDefines "" + SimCustomCompilerFlags "" + SimCustomLinkerFlags "" + SFSimEcho on + SimCtrlC on + SimIntegrity on + SimUseLocalCustomCode off + SimParseCustomCode on + SimBuildMode "sf_incremental_build" + SimGenImportedTypeDefs off + ModelFunctionsGlobalVisibility "on" + CompileTimeRecursionLimit 50 + EnableRuntimeRecursion on + MATLABDynamicMemAlloc on + MATLABDynamicMemAllocThreshold 65536 + } + Simulink.RTWCC { + $BackupClass "Simulink.RTWCC" + $ObjectID 16 + Version "1.17.0" + Array { + Type "Cell" + Dimension 13 + Cell "IncludeHyperlinkInReport" + Cell "GenerateTraceInfo" + Cell "GenerateTraceReport" + Cell "GenerateTraceReportSl" + Cell "GenerateTraceReportSf" + Cell "GenerateTraceReportEml" + Cell "GenerateWebview" + Cell "GenerateCodeMetricsReport" + Cell "GenerateCodeReplacementReport" + Cell "PortableWordSizes" + Cell "GenerateMissedCodeReplacementReport" + Cell "GenerateErtSFunction" + Cell "CreateSILPILBlock" + PropName "DisabledProps" + } + SystemTargetFile "grt.tlc" + HardwareBoard "None" + TLCOptions "" + GenCodeOnly off + MakeCommand "make_rtw" + GenerateMakefile on + PackageGeneratedCodeAndArtifacts off + PackageName "" + TemplateMakefile "grt_default_tmf" + PostCodeGenCommand "" + Description "" + GenerateReport off + SaveLog off + RTWVerbose on + RetainRTWFile off + RTWBuildHooks [] + ProfileTLC off + TLCDebug off + TLCCoverage off + TLCAssert off + RTWUseLocalCustomCode off + RTWUseSimCustomCode off + CustomSourceCode "" + CustomHeaderCode "" + CustomInclude "" + CustomSource "" + CustomLibrary "" + CustomDefine "" + CustomLAPACKCallback "" + CustomInitializer "" + CustomTerminator "" + Toolchain "Automatically locate an installed toolchain" + BuildConfiguration "Faster Builds" + CustomToolchainOptions [] + IncludeHyperlinkInReport off + LaunchReport off + PortableWordSizes off + CreateSILPILBlock "None" + CodeExecutionProfiling off + CodeExecutionProfileVariable "executionProfile" + CodeProfilingSaveOptions "SummaryOnly" + CodeProfilingInstrumentation off + SILDebugging off + TargetLang "C" + IncludeBusHierarchyInRTWFileBlockHierarchyMap off + GenerateTraceInfo off + GenerateTraceReport off + GenerateTraceReportSl off + GenerateTraceReportSf off + GenerateTraceReportEml off + GenerateWebview off + GenerateCodeMetricsReport off + GenerateCodeReplacementReport off + GenerateMissedCodeReplacementReport off + RTWCompilerOptimization "off" + ObjectivePriorities [] + RTWCustomCompilerOptimizations "" + CheckMdlBeforeBuild "Off" + SharedConstantsCachingThreshold 1024 + Array { + Type "Handle" + Dimension 2 + Simulink.CodeAppCC { + $ObjectID 17 + Version "1.17.0" + Array { + Type "Cell" + Dimension 27 + Cell "IgnoreCustomStorageClasses" + Cell "IgnoreTestpoints" + Cell "InsertBlockDesc" + Cell "InsertPolySpaceComments" + Cell "SFDataObjDesc" + Cell "MATLABFcnDesc" + Cell "SimulinkDataObjDesc" + Cell "DefineNamingRule" + Cell "SignalNamingRule" + Cell "ParamNamingRule" + Cell "InlinedPrmAccess" + Cell "CustomSymbolStr" + Cell "CustomSymbolStrGlobalVar" + Cell "CustomSymbolStrType" + Cell "CustomSymbolStrField" + Cell "CustomSymbolStrFcn" + Cell "CustomSymbolStrFcnArg" + Cell "CustomSymbolStrBlkIO" + Cell "CustomSymbolStrTmpVar" + Cell "CustomSymbolStrMacro" + Cell "ReqsInCode" + Cell "InternalIdentifier" + Cell "CustomSymbolStrModelFcn" + Cell "CustomSymbolStrUtil" + Cell "CustomUserTokenString" + Cell "CustomSymbolStrEmxType" + Cell "CustomSymbolStrEmxFcn" + PropName "DisabledProps" + } + Description "" + Comment "" + ForceParamTrailComments off + GenerateComments on + CommentStyle "Auto" + IgnoreCustomStorageClasses on + IgnoreTestpoints off + IncHierarchyInIds off + MaxIdLength 31 + PreserveName off + PreserveNameWithParent off + ShowEliminatedStatement off + OperatorAnnotations off + IncAutoGenComments off + SimulinkDataObjDesc off + SFDataObjDesc off + MATLABFcnDesc off + IncDataTypeInIds off + MangleLength 1 + CustomSymbolStrGlobalVar "$R$N$M" + CustomSymbolStrType "$N$R$M_T" + CustomSymbolStrField "$N$M" + CustomSymbolStrFcn "$R$N$M$F" + CustomSymbolStrModelFcn "$R$N" + CustomSymbolStrFcnArg "rt$I$N$M" + CustomSymbolStrBlkIO "rtb_$N$M" + CustomSymbolStrTmpVar "$N$M" + CustomSymbolStrMacro "$R$N$M" + CustomSymbolStrUtil "$N$C" + CustomSymbolStrEmxType "emxArray_$M$N" + CustomSymbolStrEmxFcn "emx$M$N" + CustomUserTokenString "" + CustomCommentsFcn "" + DefineNamingRule "None" + DefineNamingFcn "" + ParamNamingRule "None" + ParamNamingFcn "" + SignalNamingRule "None" + SignalNamingFcn "" + InsertBlockDesc off + InsertPolySpaceComments off + SimulinkBlockComments on + MATLABSourceComments off + EnableCustomComments off + InternalIdentifierFile "" + InternalIdentifier "Shortened" + InlinedPrmAccess "Literals" + ReqsInCode off + UseSimReservedNames off + ReservedNameArray [] + } + Simulink.GRTTargetCC { + $BackupClass "Simulink.TargetCC" + $ObjectID 18 + Version "1.17.0" + Array { + Type "Cell" + Dimension 18 + Cell "GeneratePreprocessorConditionals" + Cell "IncludeMdlTerminateFcn" + Cell "SupportNonInlinedSFcns" + Cell "SuppressErrorStatus" + Cell "ERTCustomFileBanners" + Cell "GenerateSampleERTMain" + Cell "GenerateTestInterfaces" + Cell "ModelStepFunctionPrototypeControlCompliant" + Cell "CPPClassGenCompliant" + Cell "PortableWordSizes" + Cell "PurelyIntegerCode" + Cell "SupportComplex" + Cell "SupportAbsoluteTime" + Cell "SupportContinuousTime" + Cell "GenerateAllocFcn" + Cell "RemoveResetFunc" + Cell "ExistingSharedCode" + Cell "RemoveDisableFunc" + PropName "DisabledProps" + } + Description "" + TargetFcnLib "ansi_tfl_table_tmw.mat" + TargetLibSuffix "" + TargetPreCompLibLocation "" + GenFloatMathFcnCalls "NOT IN USE" + TargetLangStandard "C89/C90 (ANSI)" + CodeReplacementLibrary "None" + UtilityFuncGeneration "Auto" + MultiwordTypeDef "System defined" + MultiwordLength 2048 + GenerateFullHeader on + InferredTypesCompatibility off + ExistingSharedCode "" + GenerateSampleERTMain off + GenerateTestInterfaces off + ModelReferenceCompliant on + ParMdlRefBuildCompliant on + CompOptLevelCompliant on + ConcurrentExecutionCompliant on + IncludeMdlTerminateFcn on + GeneratePreprocessorConditionals "Disable all" + CombineOutputUpdateFcns off + CombineSignalStateStructs off + SuppressErrorStatus off + ERTFirstTimeCompliant off + IncludeFileDelimiter "Auto" + ERTCustomFileBanners off + SupportAbsoluteTime on + LogVarNameModifier "rt_" + MatFileLogging on + MultiInstanceERTCode off + CodeInterfacePackaging "Nonreusable function" + SupportNonFinite on + SupportComplex on + PurelyIntegerCode off + SupportContinuousTime on + SupportNonInlinedSFcns on + RemoveDisableFunc off + RemoveResetFunc off + SupportVariableSizeSignals off + ParenthesesLevel "Nominal" + CastingMode "Nominal" + MATLABClassNameForMDSCustomization "Simulink.SoftwareTarget.GRTCustomization" + ModelStepFunctionPrototypeControlCompliant off + CPPClassGenCompliant on + AutosarCompliant off + MDXCompliant off + GRTInterface on + GenerateAllocFcn off + UseToolchainInfoCompliant on + GenerateSharedConstants on + CoderGroups [] + AccessMethods [] + LookupTableObjectStructAxisOrder "1,2,3,4,..." + UseMalloc off + ExtMode off + ExtModeStaticAlloc off + ExtModeTesting off + ExtModeStaticAllocSize 1000000 + ExtModeTransport 0 + ExtModeMexFile "ext_comm" + ExtModeMexArgs "" + ExtModeIntrfLevel "Level1" + RTWCAPISignals off + RTWCAPIParams off + RTWCAPIStates off + RTWCAPIRootIO off + GenerateASAP2 off + MultiInstanceErrorCode "Error" + } + PropName "Components" + } + } + SlCovCC.ConfigComp { + $ObjectID 19 + Version "1.17.0" + DisabledProps [] + Description "Simulink Coverage Configuration Component" + Name "Simulink Coverage" + CovEnable off + CovScope "EntireSystem" + CovIncludeTopModel on + RecordCoverage off + CovPath "/" + CovSaveName "covdata" + CovCompData "" + CovMetricSettings "dw" + CovFilter "" + CovHTMLOptions "" + CovNameIncrementing off + CovHtmlReporting on + CovForceBlockReductionOff on + CovEnableCumulative on + CovSaveCumulativeToWorkspaceVar on + CovSaveSingleToWorkspaceVar on + CovCumulativeVarName "covCumulativeData" + CovCumulativeReport off + CovSaveOutputData on + CovOutputDir "slcov_output/$ModelName$" + CovDataFileName "$ModelName$_cvdata" + CovShowResultsExplorer on + CovReportOnPause on + CovModelRefEnable "off" + CovModelRefExcluded "" + CovExternalEMLEnable off + CovSFcnEnable off + CovBoundaryAbsTol 1e-05 + CovBoundaryRelTol 0.01 + CovUseTimeInterval off + CovStartTime 0 + CovStopTime 0 + CovMcdcMode "Masking" + } + hdlcoderui.hdlcc { + $ObjectID 20 + Version "1.17.0" + DisabledProps [] + Description "HDL Coder custom configuration component" + Name "HDL Coder" + Array { + Type "Cell" + Dimension 1 + Cell " " + PropName "HDLConfigFile" + } + HDLCActiveTab "0" + } + PropName "Components" + } + Name "Configuration" + ExtraOptions "" + CurrentDlgPage "Solver" + ConfigPrmDlgPosition [ 963, 135, 1843, 765 ] + } + PropName "ConfigurationSets" + } + ExplicitPartitioning off + BlockDefaults { + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + NamePlacement "normal" + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + ShowName on + HideAutomaticName on + BlockRotation 0 + BlockMirror off + } + AnnotationDefaults { + HorizontalAlignment "center" + VerticalAlignment "middle" + ForegroundColor "black" + BackgroundColor "white" + DropShadow off + FontName "Helvetica" + FontSize 10 + FontWeight "normal" + FontAngle "normal" + UseDisplayTextAsClickCallback off + } + LineDefaults { + FontName "Helvetica" + FontSize 9 + FontWeight "normal" + FontAngle "normal" + } + MaskDefaults { + SelfModifiable "off" + IconFrame "on" + IconOpaque "opaque" + RunInitForIconRedraw "analyze" + IconRotate "none" + PortRotate "default" + IconUnits "autoscale" + } + MaskParameterDefaults { + Evaluate "on" + Tunable "on" + NeverSave "off" + Internal "off" + ReadOnly "off" + Enabled "on" + Visible "on" + ToolTip "on" + } + BlockParameterDefaults { + Block { + BlockType Clock + DisplayTime off + Decimation "10" + } + Block { + BlockType Constant + Value "1" + VectorParams1D on + SamplingMode "Sample based" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Inherit from 'Constant value'" + LockScale off + SampleTime "inf" + FramePeriod "inf" + PreserveConstantTs off + } + Block { + BlockType Demux + Outputs "4" + DisplayOption "none" + BusSelectionMode off + } + Block { + BlockType Gain + Gain "1" + Multiplication "Element-wise(K.*u)" + ParamMin "[]" + ParamMax "[]" + ParamDataTypeStr "Inherit: Same as input" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Inport + Port "1" + OutputFunctionCall off + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + Unit "inherit" + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + LatchByDelayingOutsideSignal off + LatchInputForFeedbackSignals off + Interpolate on + } + Block { + BlockType Outport + Port "1" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: auto" + LockScale off + BusOutputAsStruct off + Unit "inherit" + PortDimensions "-1" + VarSizeSig "Inherit" + SampleTime "-1" + SignalType "auto" + SamplingMode "auto" + EnsureOutportIsVirtual off + SourceOfInitialOutputValue "Dialog" + OutputWhenDisabled "held" + InitialOutput "[]" + MustResolveToSignalObject off + } + Block { + BlockType Product + Inputs "2" + Multiplication "Element-wise(.*)" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Zero" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType S-Function + FunctionName "system" + SFunctionModules "''" + PortCounts "[]" + } + Block { + BlockType SubSystem + ShowPortLabels "FromPortIcon" + Permissions "ReadWrite" + PermitHierarchicalResolution "All" + TreatAsAtomicUnit off + MinAlgLoopOccurrences off + PropExecContextOutsideSubsystem off + SystemSampleTime "-1" + RTWSystemCode "Auto" + RTWFcnNameOpts "Auto" + RTWFileNameOpts "Auto" + FunctionInterfaceSpec "void_void" + FunctionWithSeparateData off + RTWMemSecFuncInitTerm "Inherit from model" + RTWMemSecFuncExecute "Inherit from model" + RTWMemSecDataConstants "Inherit from model" + RTWMemSecDataInternal "Inherit from model" + RTWMemSecDataParameters "Inherit from model" + SimViewingDevice off + DataTypeOverride "UseLocalSettings" + DataTypeOverrideAppliesTo "AllNumericTypes" + MinMaxOverflowLogging "UseLocalSettings" + Opaque off + MaskHideContents off + SFBlockType "NONE" + GeneratePreprocessorConditionals off + PropagateVariantConditions off + TreatAsGroupedWhenPropagatingVariantConditions on + ContentPreviewEnabled off + IsWebBlock off + } + Block { + BlockType Sum + IconShape "rectangular" + Inputs "++" + CollapseMode "All dimensions" + CollapseDim "1" + InputSameDT on + AccumDataTypeStr "Inherit: Inherit via internal rule" + OutMin "[]" + OutMax "[]" + OutDataTypeStr "Inherit: Same as first input" + LockScale off + RndMeth "Floor" + SaturateOnIntegerOverflow on + SampleTime "-1" + } + Block { + BlockType Terminator + } + } + System { + Name "WBToolboxLibrary_repository" + Location [2086, 89, 3681, 955] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + ModelBrowserWidth 200 + ScreenColor "black" + PaperOrientation "landscape" + PaperPositionMode "auto" + PaperType "usletter" + PaperUnits "inches" + TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] + TiledPageScale 1 + ShowPageBoundaries off + ZoomFactor "343" + ReportName "simulink-default.rpt" + SIDHighWatermark "1790" + Block { + BlockType SubSystem + Name "Utilities" + SID "192" + Ports [] + Position [364, 17, 462, 114] + ZOrder -1 + ForegroundColor "white" + DropShadow on + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 21 + $ClassName "Simulink.Mask" + Display "image(imread('utilities.png'))" + } + System { + Name "Utilities" + Location [2086, 89, 3681, 955] + Open on + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "213" + Block { + BlockType SubSystem + Name "Configuration" + SID "1774" + Ports [] + Position [435, 260, 510, 290] + ZOrder 95 + InitFcn "source = get_param(gcb,'ConfigSource');\n\nif strcmp(source, 'Workspace')\n object = get_param(gcb,'" + "ConfigObject');\n WBToolbox.WBToolboxConfig2Mask(gcb, object);\nend\n\nWBToolbox.Mask2WBToolboxConfig(gcb);\n\nc" + "lear object source" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 22 + $ClassName "Simulink.Mask" + SelfModifiable "on" + Display "disp('Config')" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 8 + Object { + $ObjectID 23 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Workspace" + Cell "Mask" + PropName "TypeOptions" + } + Name "ConfigSource" + Prompt "Configuration from" + Value "Mask" + Callback "% Get the Workspace/Mask menu\nh = Simulink.Mask.get(gcb);\ncurrentConfigSource = get_param(gcb,'ConfigSo" + "urce');\n\nif strcmp(currentConfigSource,'Workspace')\n % Switch the visibility of the GUI elements\n set_para" + "m(gcb,'MaskVisibilities',{'on';'on';'on';'on';'on';'on';'on';'on';});\n h.Parameters(3).ReadOnly = 'on';\n h.P" + "arameters(4).ReadOnly = 'on';\n h.Parameters(5).ReadOnly = 'on';\n h.Parameters(6).ReadOnly = 'on';\n h.Par" + "ameters(7).ReadOnly = 'on';\n h.Parameters(8).ReadOnly = 'on';\n \n % Parse the object inserted in the Conf" + "igObject field\n currentConfigObject = get_param(gcb,'ConfigObject');\n WBToolbox.WBToolboxConfig2Mask(gcb,cur" + "rentConfigObject);\n clear currentConfigObject;\nelseif strcmp(currentConfigSource,'Mask')\n % Switch the visi" + "bility of the GUI elements\n set_param(gcb,'MaskVisibilities',{'on';'off';'on';'on';'on';'on';'on';'on';});\n " + "h.Parameters(3).ReadOnly = 'off';\n h.Parameters(4).ReadOnly = 'off';\n h.Parameters(5).ReadOnly = 'off';\n " + " h.Parameters(6).ReadOnly = 'off';\n h.Parameters(7).ReadOnly = 'off';\n h.Parameters(8).ReadOnly = 'off';\n " + " h.Parameters(3).Enabled = 'on';\n h.Parameters(4).Enabled = 'on';\n h.Parameters(5).Enabled = 'on';\n h.P" + "arameters(6).Enabled = 'on';\n h.Parameters(7).Enabled = 'on';\n h.Parameters(8).Enabled = 'on';\nend\n\nclear" + " h currentConfigSource" + } + Object { + $ObjectID 24 + Type "edit" + Name "ConfigObject" + Prompt "Name of the object" + Value "'WBTConfigRobot'" + Tunable "off" + Visible "off" + Callback "% This code get called whatsoever\nif strcmp(char(get_param(gcb,'ConfigSource')),'Mask')\n return\nend" + "\n\n% Parse the object inserted in the ConfigObject field\ncurrentConfigObject = get_param(gcb,'ConfigObject');\nWBT" + "oolbox.WBToolboxConfig2Mask(gcb,currentConfigObject);\n\nclear currentConfigObject;" + } + Object { + $ObjectID 25 + Type "edit" + Name "RobotName" + Prompt "Robot Name" + Value "'icub'" + } + Object { + $ObjectID 26 + Type "edit" + Name "UrdfFile" + Prompt "Urdf File" + Value "'model.urdf'" + } + Object { + $ObjectID 27 + Type "edit" + Name "ControlledJoints" + Prompt "Controlled Joints" + Value "{'l_elbow','l_shoulder_pitch','torso_roll'}" + } + Object { + $ObjectID 28 + Type "edit" + Name "ControlBoardsNames" + Prompt "Control Boards Names" + Value "{'left_arm','right_arm','torso'}" + } + Object { + $ObjectID 29 + Type "edit" + Name "LocalName" + Prompt "Local Name" + Value "'WBT'" + } + Object { + $ObjectID 30 + Type "edit" + Name "GravityVector" + Prompt "Gravity Vector" + Value "[0,0,-9.81]" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 31 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 32 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 33 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 34 + $ClassName "Simulink.dialog.TabContainer" + Array { + Type "Simulink.dialog.Tab" + Dimension 2 + Object { + $ObjectID 35 + Prompt "From" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 2 + Object { + $ObjectID 36 + $ClassName "Simulink.dialog.parameter.Popup" + Name "ConfigSource" + } + Object { + $ObjectID 37 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "ConfigObject" + } + PropName "DialogControls" + } + Name "TabFrom" + } + Object { + $ObjectID 38 + Prompt "Data" + Array { + Type "Simulink.dialog.parameter.Edit" + Dimension 6 + Object { + $ObjectID 39 + PromptLocation "left" + Name "RobotName" + } + Object { + $ObjectID 40 + PromptLocation "left" + Name "UrdfFile" + } + Object { + $ObjectID 41 + PromptLocation "left" + Name "ControlledJoints" + } + Object { + $ObjectID 42 + PromptLocation "left" + Name "ControlBoardsNames" + } + Object { + $ObjectID 43 + PromptLocation "left" + Name "LocalName" + } + Object { + $ObjectID 44 + PromptLocation "left" + Name "GravityVector" + } + PropName "DialogControls" + } + Name "TabData" + } + PropName "DialogControls" + } + Name "TabContainer" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + System { + Name "Configuration" + Location [550, 86, 1677, 725] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "600" + SIDHighWatermark "70" + Block { + BlockType Constant + Name "ImConfig" + SID "1774:67" + Position [20, 20, 50, 50] + ZOrder 81 + Value "0" + } + Block { + BlockType Terminator + Name "Terminator" + SID "1774:68" + Position [95, 25, 115, 45] + ZOrder 80 + } + Line { + ZOrder 1 + SrcBlock "ImConfig" + SrcPort 1 + DstBlock "Terminator" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "DampPinv" + SID "104" + Ports [2, 1] + Position [435, 153, 505, 197] + ZOrder -1 + BackgroundColor "[0.848000, 0.128048, 0.320035]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 45 + $ClassName "Simulink.Mask" + Object { + $PropName "Parameters" + $ObjectID 46 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "tol" + Prompt "Tolerance" + Value "1e-4" + } + } + System { + Name "DampPinv" + Location [0, 29, 1280, 744] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "100" + Block { + BlockType Inport + Name "mat" + SID "105" + Position [50, 53, 80, 67] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "sigma" + SID "106" + Position [50, 93, 80, 107] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Damped Pseudo Inverse" + SID "107" + Ports [2, 1] + Position [105, 39, 200, 121] + ZOrder -4 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + Variant off + System { + Name "Damped Pseudo Inverse" + Location [12, 45, 1279, 3773] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "100" + SIDHighWatermark "1622" + Block { + BlockType Inport + Name "mat" + SID "107::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "sigma" + SID "107::25" + Position [20, 136, 40, 154] + ZOrder 11 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "107::1621" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 101 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "107::1620" + Tag "Stateflow S-Function WBToolboxLibrary_repository 6" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 100 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "DPinv" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "107::1622" + Position [460, 241, 480, 259] + ZOrder 102 + } + Block { + BlockType Outport + Name "DPinv" + SID "107::5" + Position [460, 101, 480, 119] + ZOrder -6 + IconDisplay "Port number" + } + Line { + ZOrder 66 + SrcBlock "mat" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 67 + SrcBlock "sigma" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "DPinv" + ZOrder 68 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "DPinv" + DstPort 1 + } + Line { + ZOrder 69 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 70 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "DPinv" + SID "108" + Position [225, 73, 255, 87] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "sigma" + SrcPort 1 + DstBlock "Damped Pseudo Inverse" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "Damped Pseudo Inverse" + SrcPort 1 + DstBlock "DPinv" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "mat" + SrcPort 1 + DstBlock "Damped Pseudo Inverse" + DstPort 1 + } + } + } + Block { + BlockType S-Function + Name "DiscreteFilter" + SID "1790" + Ports [1, 1] + Position [125, 260, 185, 290] + ZOrder 96 + BackgroundColor "yellow" + FunctionName "WBToolbox" + Parameters "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 47 + $ClassName "Simulink.Mask" + Type "Discrete Filter" + Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." + Initialization "filterType = get_param(gcb, 'filterType');\nnumCoeffs = mat2str(numCoeffs);\ndenCoeffs = ma" + "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n y0 = mat2str" + "(y0);\n u0 = mat2str(u0);\nelse\n y0 = 'none';\n u0 = 'none';\nend\n\nclear initStatus" + Display "disp('Filter')" + Array { + Type "Simulink.MaskParameter" + Dimension 9 + Object { + $ObjectID 48 + Type "popup" + Array { + Type "Cell" + Dimension 3 + Cell "Generic" + Cell "FirstOrderLowPassFilter" + Cell "MedianFilter" + PropName "TypeOptions" + } + Name "filterType" + Prompt "Type of the filter" + Value "Generic" + Callback "% From: https://it.mathworks.com/help/simulink/slref/simulink.mask-class.html\nfilterType = get_param(gcb" + ", 'filterType');\ninitStatus = get_param(gcb, 'initStatus');\np = Simulink.Mask.get(gcbh);\nhowToCoeffs = p.getDialo" + "gControl('howToCoeffs');\n\n%set_param(gcb, 'initStatus','off');\nif (strcmp(initStatus,'on'))\n vis_init = 'on';" + "\nelse\n vis_init = 'off';\nend\n\nif(strcmp(filterType, 'Generic'))\n set_param(gcb, 'MaskVisibilities',{'on'" + ",'on','on','off','off','off','on',vis_init,vis_init});\n howToCoeffs.Visible = 'on';\nelseif(strcmp(filterType, '" + "FirstOrderLowPassFilter'))\n set_param(gcb, 'MaskVisibilities',{'on','off','off','on','on','off','on',vis_init,'o" + "ff'});\n howToCoeffs.Visible = 'off';\nelseif(strcmp(filterType, 'MedianFilter'))\n set_param(gcb, 'MaskVisibi" + "lities',{'on','off','off','off','off','on','on',vis_init,'off'});\n howToCoeffs.Visible = 'off';\nend\n\nclear fi" + "lterType initStatus p howToCoeffs vis_init;" + } + Object { + $ObjectID 49 + Type "edit" + Name "numCoeffs" + Prompt "Numerator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 50 + Type "edit" + Name "denCoeffs" + Prompt "Denominator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 51 + Type "edit" + Name "Fc" + Prompt "Cut Frequency (Hz)" + Value "0" + Visible "off" + } + Object { + $ObjectID 52 + Type "edit" + Name "Ts" + Prompt "Sampling time (s)" + Value "0" + Visible "off" + } + Object { + $ObjectID 53 + Type "edit" + Name "orderMedianFilter" + Prompt "Order" + Value "0" + Visible "off" + } + Object { + $ObjectID 54 + Type "checkbox" + Name "initStatus" + Prompt "Define initial conditions" + Value "off" + Callback "initStatus = get_param(gcb, 'initStatus');\nvisibilities = get_param(gcb, 'MaskVisibilities');\nfilterT" + "ype = get_param(gcb, 'filterType');\n\nif (strcmp(initStatus,'off'))\n visibilities{8} = 'off';\n visibiliti" + "es{9} = 'off';\nelseif (strcmp(initStatus,'on'))\n visibilities{8} = 'on';\n if (strcmp(filterType,'Generic'))" + "\n visibilities{9} = 'on';\n end\nend\n\nset_param(gcb, 'MaskVisibilities', visibilities);\n\nclear initSt" + "atus visibilities filterType;" + } + Object { + $ObjectID 55 + Type "edit" + Name "y0" + Prompt "Output y0" + Value "[0]" + Visible "off" + } + Object { + $ObjectID 56 + Type "edit" + Name "u0" + Prompt "Input u0" + Value "[0]" + Visible "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 57 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 58 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 59 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 8 + Object { + $ObjectID 60 + $ClassName "Simulink.dialog.parameter.Popup" + Name "filterType" + } + Object { + $ObjectID 61 + $ClassName "Simulink.dialog.parameter.Edit" + Name "numCoeffs" + } + Object { + $ObjectID 62 + $ClassName "Simulink.dialog.parameter.Edit" + Name "denCoeffs" + } + Object { + $ObjectID 63 + $ClassName "Simulink.dialog.Text" + Prompt "* The coefficients are ordered in increasing power of z^-1" + Name "howToCoeffs" + } + Object { + $ObjectID 64 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Fc" + } + Object { + $ObjectID 65 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Ts" + } + Object { + $ObjectID 66 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "orderMedianFilter" + } + Object { + $ObjectID 67 + $ClassName "Simulink.dialog.Group" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 68 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "initStatus" + } + Object { + $ObjectID 69 + $ClassName "Simulink.dialog.parameter.Edit" + Name "y0" + } + Object { + $ObjectID 70 + $ClassName "Simulink.dialog.parameter.Edit" + Name "u0" + } + PropName "DialogControls" + } + Name "Container3" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } + Block { + BlockType S-Function + Name "DoFs converter" + SID "1771" + Ports [1, 3] + Position [240, 241, 385, 309] + ZOrder 81 + FunctionName "WBToolbox" + Parameters "'ModelPartitioner',WBTConfigParameters,configBlockAbsName,yarp2WBI" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 71 + $ClassName "Simulink.Mask" + Type "DoFs converter" + Description "This block converts from a DoFs serialization to a YARP serialization or viceversa depending on" + " its configuration.\nThe robot name is taken from the environment variable YARP_ROBOT_NAME.\n\nParameters:\n\n- C" + "onversion direction: specify if you want to convert from YARP to the current DoFs serialization or viceversa.\n" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend\n\n% ModelPartitioner Initialization\n% ===============================\n\nswitch" + " get_param(gcb, 'yarp2WBIOption')\n case 'From YARP to WBI'\n yarp2WBI = true;\n case 'From WBI to Y" + "ARP'\n yarp2WBI = false;\n otherwise\nend" + Display "robotName = getenv('YARP_ROBOT_NAME');\n\ncurrentSelection = get_param(gcb,'yarp2WBIoption');\n\nif" + " strcmp(currentSelection,'From YARP to WBI')\n direction = 'YARP -> DoFs';\nelse\n direction = 'DoFs -> YAR" + "P';\nend\n\nfprintf('%s\\n\\n%s', robotName, direction);" + RunInitForIconRedraw "off" + Object { + $PropName "Parameters" + $ObjectID 72 + $ClassName "Simulink.MaskParameter" + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "From YARP to WBI" + Cell "From WBI to YARP" + PropName "TypeOptions" + } + Name "yarp2WBIOption" + Prompt "Conversion Direction" + Value "From YARP to WBI" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 73 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 74 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 75 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 76 + $ClassName "Simulink.dialog.parameter.Popup" + Name "yarp2WBIOption" + } + Object { + $ObjectID 77 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 78 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } + Block { + BlockType S-Function + Name "Minimum Jerk Trajectory Generator" + SID "1747" + Ports [1, 3] + Position [450, 73, 605, 127] + ZOrder 78 + FunctionName "WBToolbox" + Parameters "'MinimumJerkTrajectoryGenerator',sampleTime, settlingTime, firstDerivatives, secondDerivatives, expl" + "icitInitialValue, externalSettlingTime,resetOnSettlingTime" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 79 + $ClassName "Simulink.Mask" + Type "Minimum Jerk Trajectory Generator" + Description "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk " + "Trajectory Generator is approximated using a \n3rd order LTI dynamical system \n(for more details see [1]). \nPos" + "ition, velocity and acceleration trajectories are computed.\n The main advantage with respect to the standard pol" + "ynomial \nform is that if the reference value yd changes \nthere is no need to recompute the filter coefficients." + "\n\n[1] Pattacini, U.; Nori, F.; Natale, L.; Metta, G.; Sandini, G., \"An experimental evaluation of a novel mini" + "mum-jerk cartesian controller for humanoid robots,\" in Intelligent Robots and Systems (IROS), 2010 IEEE/RSJ Inte" + "rnational Conference on , vol., no., pp.1668-1674, 18-22 Oct. 2010\ndoi: 10.1109/IROS.2010.5650851\nURL: http://i" + "eeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=5650851&isnumber=5648787" + Display "firstDer = get_param(gcb, 'firstDerivatives');\nsecondDer = get_param(gcb, 'secondDerivatives');\ni" + "nitialValues = get_param(gcb, 'explicitInitialValue');\nexternalSettlingTimeParam = get_param(gcb, 'externalSettl" + "ingTime');\n\n%Inputs\nportIndex = 2;\nport_label('input', 1, 'Reference')\nif(strcmp(initialValues, 'on'))\n " + "port_label('input', portIndex, 'Initial Value')\n portIndex = portIndex + 1;\nend\n\nif(strcmp(externalSettlin" + "gTimeParam, 'on'))\n port_label('input', portIndex, 'Settling Time')\n portIndex = portIndex + 1;\nend\n\n%" + "Outputs\nport_label('output', 1, 'Signal')\nsecondDerPortIndex = 2;\nif(strcmp(firstDer, 'on'))\n port_label('" + "output', 2, 'First Derivative')\n secondDerPortIndex = secondDerPortIndex + 1;\nend\nif(strcmp(secondDer, 'on'" + "))\n port_label('output', secondDerPortIndex, 'Second Derivative')\nend\n\n\n" + Array { + Type "Simulink.MaskParameter" + Dimension 7 + Object { + $ObjectID 80 + Type "checkbox" + Name "externalSettlingTime" + Prompt "External Settling Time" + Value "off" + Callback "externalSettlingTime = get_param(gcb, 'externalSettlingTime');\nvisibility = get_param(gcb, 'MaskVisibili" + "ties');\nif(strcmp(externalSettlingTime, 'on'))\n visibility{2} = 'off';\n visibility{4} = 'on';\nelse\n vi" + "sibility{2} = 'on';\n visibility{4} = 'off';\nend\nset_param(gcb, 'MaskVisibilities',visibility);\nclear external" + "SettlingTime" + } + Object { + $ObjectID 81 + Type "edit" + Name "settlingTime" + Prompt "Settling Time" + Value "3" + } + Object { + $ObjectID 82 + Type "edit" + Name "sampleTime" + Prompt "Sample Time" + Value "0.01" + } + Object { + $ObjectID 83 + Type "checkbox" + Name "resetOnSettlingTime" + Prompt "Reset on Settling Time Changes" + Value "off" + } + Object { + $ObjectID 84 + Type "checkbox" + Name "firstDerivatives" + Prompt "Output First Derivative" + Value "on" + } + Object { + $ObjectID 85 + Type "checkbox" + Name "secondDerivatives" + Prompt "Output Second Derivative" + Value "on" + } + Object { + $ObjectID 86 + Type "checkbox" + Name "explicitInitialValue" + Prompt "Explicit Initial Value" + Value "off" + Visible "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 87 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 88 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 89 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 90 + $ClassName "Simulink.dialog.TabContainer" + Array { + Type "Simulink.dialog.Tab" + Dimension 2 + Object { + $ObjectID 91 + Prompt "Trajectory Parameters" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 4 + Object { + $ObjectID 92 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "externalSettlingTime" + } + Object { + $ObjectID 93 + $ClassName "Simulink.dialog.parameter.Edit" + Name "settlingTime" + } + Object { + $ObjectID 94 + $ClassName "Simulink.dialog.parameter.Edit" + Name "sampleTime" + } + Object { + $ObjectID 95 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "resetOnSettlingTime" + } + PropName "DialogControls" + } + Name "Tab1" + } + Object { + $ObjectID 96 + Prompt "Input/Output" + Array { + Type "Simulink.dialog.parameter.CheckBox" + Dimension 3 + Object { + $ObjectID 97 + Name "firstDerivatives" + } + Object { + $ObjectID 98 + Name "secondDerivatives" + } + Object { + $ObjectID 99 + Name "explicitInitialValue" + } + PropName "DialogControls" + } + Name "Tab2" + } + PropName "DialogControls" + } + Name "Container3" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } + Block { + BlockType S-Function + Name "Real Time Synchronizer" + SID "1657" + Ports [] + Position [155, 14, 280, 51] + ZOrder 23 + ForegroundColor "[0.917647, 0.917647, 0.917647]" + BackgroundColor "gray" + ShowName off + FunctionName "WBToolbox" + Parameters "'RealTimeSynchronizer',period" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 100 + $ClassName "Simulink.Mask" + Type "Real Time Synchronizer" + Description "This block slows down the simulation trying to match the period specified \nas parameter (in se" + "conds).\nThe bigger the period the more probable \nis that Simulink can remain synched with it.\n" + Display "disp('Real Time Synchronizer')" + Object { + $PropName "Parameters" + $ObjectID 101 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "period" + Prompt "Controller Period (in seconds)" + Value "0.01" + } + } + } + Block { + BlockType S-Function + Name "Simulator Synchronizer" + SID "1658" + Ports [] + Position [335, 14, 465, 51] + ZOrder 24 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + FunctionName "WBToolbox" + Parameters "'SimulatorSynchronizer',period, serverPortName, clientPortName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 102 + $ClassName "Simulink.Mask" + Type "Simulator Synchronizer" + Description "This block synchronizes with the external simulation on a simulator \n(only Gazebo is supported" + " at the moment).\n\n" + Display "disp('Simulator Synchronizer')" + Array { + Type "Simulink.MaskParameter" + Dimension 3 + Object { + $ObjectID 103 + Type "edit" + Name "period" + Prompt "Controller Period (in seconds)" + Value "0.01" + } + Object { + $ObjectID 104 + Type "edit" + Name "serverPortName" + Prompt "Server Port Name" + Value "'/clock/rpc'" + } + Object { + $ObjectID 105 + Type "edit" + Name "clientPortName" + Prompt "Client Port Name" + Value "'/WBT_synchronizer/rpc:o'" + } + PropName "Parameters" + } + } + } + Block { + BlockType SubSystem + Name "TruncPinv" + SID "109" + Ports [2, 1] + Position [335, 153, 405, 197] + ZOrder -3 + BackgroundColor "[0.534601, 0.470279, 1.000000]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 106 + $ClassName "Simulink.Mask" + Object { + $PropName "Parameters" + $ObjectID 107 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "tol" + Prompt "Tolerance" + Value "1e-4" + } + } + System { + Name "TruncPinv" + Location [0, 29, 1280, 744] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "100" + Block { + BlockType Inport + Name "S" + SID "110" + Position [50, 53, 80, 67] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tol" + SID "111" + Position [50, 93, 80, 107] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType SubSystem + Name "Truncated PseudoInverse" + SID "112" + Ports [2, 1] + Position [105, 39, 200, 121] + ZOrder -4 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + Variant off + System { + Name "Truncated PseudoInverse" + Location [12, 45, 1279, 3773] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "100" + SIDHighWatermark "1613" + Block { + BlockType Inport + Name "mat" + SID "112::1" + Position [20, 101, 40, 119] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "tol" + SID "112::25" + Position [20, 136, 40, 154] + ZOrder 11 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "112::1612" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 101 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "112::1611" + Tag "Stateflow S-Function WBToolboxLibrary_repository 7" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 100 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "TPinv" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "112::1613" + Position [460, 241, 480, 259] + ZOrder 102 + } + Block { + BlockType Outport + Name "TPinv" + SID "112::5" + Position [460, 101, 480, 119] + ZOrder -6 + IconDisplay "Port number" + } + Line { + ZOrder 66 + SrcBlock "mat" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 67 + SrcBlock "tol" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "TPinv" + ZOrder 68 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "TPinv" + DstPort 1 + } + Line { + ZOrder 69 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 70 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "Tpinv" + SID "113" + Position [225, 73, 255, 87] + ZOrder -5 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S" + SrcPort 1 + DstBlock "Truncated PseudoInverse" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Truncated PseudoInverse" + SrcPort 1 + DstBlock "Tpinv" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "tol" + SrcPort 1 + DstBlock "Truncated PseudoInverse" + DstPort 2 + } + } + } + Block { + BlockType S-Function + Name "Yarp Clock" + SID "1773" + Ports [0, 1] + Position [335, 76, 405, 124] + ZOrder 85 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + FunctionName "WBToolbox" + Parameters "'YarpClock'" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 108 + $ClassName "Simulink.Mask" + Type "YARP Clock" + Description "This block outputs the current YARP Time.\nIn a nutshell, this block outputs the equivalent of " + "the C++ function call yarp::os::Time::now()" + SelfModifiable "on" + Display "disp('YARP Time')\n" + } + } + Block { + BlockType S-Function + Name "Yarp Read" + SID "1632" + Ports [0, 2] + Position [105, 74, 165, 121] + ZOrder 22 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + FunctionName "WBToolbox" + Parameters "'YarpRead',portName,signalSize,blocking,timestamp,autoconnect,errorOnConnection" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 109 + $ClassName "Simulink.Mask" + Type "YARP Read" + Description "This block behaves as the command 'yarp read'. \nBy default it has the option 'Autoconnect' act" + "ive, which means that the user \ncan only specify the name of the port ('Source Port Name') from which readings a" + "re desired,\nalong with the size of the expected data (e.g. 3 when reading the torso state).\nWhen the user unche" + "cks 'Autoconnect' 'Opened Port Name' field shows up \ncorresponding to the name of the port the block will create" + " \nbut won't connect to anything. \nAt this point the output will be always zero, until the user connects it\nto " + "some other port from command line by issuing 'yarp connect [from] [dest]'.\nIn this case an additional output, ca" + "lled 'signal' will be added which will output\n1 if the port has received at least one data, or 0 otherwise.\n" + SelfModifiable "on" + Display "port_label('output', 1, 'signal');\nportNumber = 1;\nif (timestamp)\n portNumber = portNumber + " + "1;\n port_label('output', portNumber, 'timestamp');\nend\n\nif (~autoconnect)\n portNumber = portNumber + 1" + ";\n port_label('output', portNumber, 'status');\nend\n\n" + Array { + Type "Simulink.MaskParameter" + Dimension 6 + Object { + $ObjectID 110 + Type "edit" + Name "portName" + Prompt "Source Port Name" + Value "'/portname'" + } + Object { + $ObjectID 111 + Type "edit" + Name "signalSize" + Prompt "Port Size" + Value "1" + } + Object { + $ObjectID 112 + Type "checkbox" + Name "blocking" + Prompt "Blocking Read" + Value "off" + } + Object { + $ObjectID 113 + Type "checkbox" + Name "timestamp" + Prompt "Read Timestamp" + Value "on" + } + Object { + $ObjectID 114 + Type "checkbox" + Name "autoconnect" + Prompt "Autoconnect" + Value "on" + Callback "autoconnect_val = get_param(gcb, 'Autoconnect');\nprompt_string = get_param(gcb, 'MaskPrompts');\nif(strc" + "mp(autoconnect_val, 'on'))\n prompt_string{1} = 'Source Port Name';\n set_param(gcb, 'MaskVisibilities',{'on';" + "'on';'on';'on';'on';'on'});\nelse\n prompt_string{1} = 'Opened Port Name';\n set_param(gcb, 'MaskVisibilities'" + ",{'on';'on';'on';'on';'on';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val promp" + "t_string" + } + Object { + $ObjectID 115 + Type "checkbox" + Name "errorOnConnection" + Prompt "Error on missing connection" + Value "on" + } + PropName "Parameters" + } + } + } + Block { + BlockType S-Function + Name "Yarp Write" + SID "1659" + Ports [1] + Position [230, 76, 290, 124] + ZOrder 27 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + FunctionName "WBToolbox" + Parameters "'YarpWrite',portName,autoconnect,errorOnConnection" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 116 + $ClassName "Simulink.Mask" + Type "YARP Write" + Description "This block behaves as the command 'yarp write'. \n\nBy default this block opens a port names as" + " the \"Opened Port Name\" parameter\nand stream the input signal to that port.\nIf the option \"Autoconnect\" is " + "specified, the first parameter become the\nname of the target port to which the data will be stramed, \ne.g. like" + " \"yarp write ... /destinationPort\"\n" + SelfModifiable "on" + Display "disp('Yarp Write')" + Array { + Type "Simulink.MaskParameter" + Dimension 3 + Object { + $ObjectID 117 + Type "edit" + Name "portName" + Prompt "Opened Port Name" + Value "'/portname'" + } + Object { + $ObjectID 118 + Type "checkbox" + Name "autoconnect" + Prompt "Autoconnect" + Value "off" + Callback "autoconnect_val = get_param(gcb, 'Autoconnect');\nprompt_string = get_param(gcb, 'MaskPrompts');\nif(strc" + "mp(autoconnect_val, 'on'))\n prompt_string{1} = 'Destination Port Name';\n set_param(gcb, 'MaskVisibilities',{" + "'on';'on';'on'});\nelse\n prompt_string{1} = 'Opened Port Name';\n set_param(gcb, 'MaskVisibilities',{'on';'on" + "';'off'});\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string" + } + Object { + $ObjectID 119 + Type "checkbox" + Name "errorOnConnection" + Prompt "Error on missing connection" + Value "on" + Visible "off" + } + PropName "Parameters" + } + } + } + Block { + BlockType SubSystem + Name "errors" + SID "714" + Ports [2, 2] + Position [200, 153, 260, 197] + ZOrder -9 + BackgroundColor "[0.300000, 0.580000, 1.000000]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 120 + $ClassName "Simulink.Mask" + Type "Errors" + Description "Computes two kinds of errors. The first is just the difference between x\nand y while the secon" + "d is the ratio (x-y)/y." + } + System { + Name "errors" + Location [0, 29, 1280, 744] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "100" + Block { + BlockType Inport + Name "x" + SID "715" + Position [30, 28, 60, 42] + ZOrder -1 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "y" + SID "716" + Position [25, 103, 55, 117] + ZOrder -2 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Sum + Name "Add" + SID "717" + Ports [2, 1] + Position [95, 27, 125, 58] + ZOrder -3 + Inputs "+-" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType Product + Name "Divide" + SID "718" + Ports [2, 1] + Position [165, 37, 195, 68] + ZOrder -4 + Inputs "*/" + InputSameDT off + OutDataTypeStr "Inherit: Inherit via internal rule" + RndMeth "Floor" + SaturateOnIntegerOverflow off + } + Block { + BlockType Outport + Name "x-y" + SID "719" + Position [225, 13, 255, 27] + ZOrder -5 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "(x-y)./y" + SID "720" + Position [220, 48, 250, 62] + ZOrder -6 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Add" + SrcPort 1 + Points [10, 0] + Branch { + ZOrder 2 + DstBlock "Divide" + DstPort 1 + } + Branch { + ZOrder 3 + Points [0, -25] + DstBlock "x-y" + DstPort 1 + } + } + Line { + ZOrder 4 + SrcBlock "x" + SrcPort 1 + DstBlock "Add" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "y" + SrcPort 1 + Points [15, 0] + Branch { + ZOrder 6 + Points [0, -60] + DstBlock "Add" + DstPort 2 + } + Branch { + ZOrder 7 + Points [60, 0; 0, -50] + DstBlock "Divide" + DstPort 2 + } + } + Line { + ZOrder 8 + SrcBlock "Divide" + SrcPort 1 + DstBlock "(x-y)./y" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "holder\n" + SID "1296" + Ports [1, 1] + Position [105, 152, 165, 198] + ZOrder 14 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 121 + $ClassName "Simulink.Mask" + Type "Holder" + Description "This block holds the first input value during the simulation." + } + System { + Name "holder\n" + Location [12, 45, 1340, 980] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "100" + Block { + BlockType Inport + Name "s" + SID "1297" + Position [145, 43, 175, 57] + ZOrder 13 + IconDisplay "Port number" + } + Block { + BlockType Clock + Name "Clock" + SID "1298" + Position [45, 65, 65, 85] + ZOrder 11 + } + Block { + BlockType Reference + Name "Compare\nTo Constant" + SID "1299" + Ports [1, 1] + Position [90, 60, 120, 90] + ZOrder 10 + LibraryVersion "1.391" + SourceBlock "simulink/Logic and Bit\nOperations/Compare\nTo Constant" + SourceType "Compare To Constant" + SourceProductName "Simulink" + SourceProductBaseCode "SL" + ContentPreviewEnabled off + relop "==" + const "0" + OutDataTypeStr "boolean" + ZeroCross on + } + Block { + BlockType SubSystem + Name "MATLAB Function" + SID "1300" + Ports [2, 1] + Position [235, 37, 305, 88] + ZOrder 15 + LibraryVersion "1.32" + ErrorFcn "Stateflow.Translate.translate" + PermitHierarchicalResolution "ExplicitOnly" + TreatAsAtomicUnit on + RequestExecContextInheritance off + SFBlockType "MATLAB Function" + Variant off + System { + Name "MATLAB Function" + Location [12, 45, 1135, 3068] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "100" + SIDHighWatermark "1619" + Block { + BlockType Inport + Name "s" + SID "1300::24" + Position [20, 101, 40, 119] + ZOrder 10 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "unused" + SID "1300::26" + Position [20, 136, 40, 154] + ZOrder 12 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Demux + Name " Demux " + SID "1300::1612" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 89 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "1300::1611" + Tag "Stateflow S-Function WBToolboxLibrary_repository 1" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 88 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "s0" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "1300::1613" + Position [460, 241, 480, 259] + ZOrder 90 + } + Block { + BlockType Outport + Name "s0" + SID "1300::25" + Position [460, 101, 480, 119] + ZOrder 11 + IconDisplay "Port number" + } + Line { + ZOrder 66 + SrcBlock "s" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 67 + SrcBlock "unused" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "s0" + ZOrder 68 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "s0" + DstPort 1 + } + Line { + ZOrder 69 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 70 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } + } + Block { + BlockType Outport + Name "s(0)" + SID "1301" + Position [330, 58, 360, 72] + ZOrder 14 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Clock" + SrcPort 1 + DstBlock "Compare\nTo Constant" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "MATLAB Function" + SrcPort 1 + DstBlock "s(0)" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "s" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "Compare\nTo Constant" + SrcPort 1 + DstBlock "MATLAB Function" + DstPort 2 + } + } + } + } + } + Block { + BlockType SubSystem + Name "wholeBodyActuators" + SID "224" + Ports [] + Position [250, 16, 348, 113] + ZOrder -17 + ForegroundColor "white" + DropShadow on + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 122 + $ClassName "Simulink.Mask" + Display "image(imread('wholeBodyActuators.png'),'center')" + } + System { + Name "wholeBodyActuators" + Location [2245, 90, 3840, 956] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "300" + Block { + BlockType SubSystem + Name "Set LowLevel PIDs" + SID "1752" + Ports [] + Position [760, 230, 835, 280] + ZOrder 41 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 123 + $ClassName "Simulink.Mask" + Type "Set Low Level PIDs" + Description "This block sets the low level PID values for the robot actuators.\n\nThe PIDs can be stored in " + "a WBToolbox.WBTPIDConfig object, each of them constructed from a WBToolbox.PID class.\nThe number of PIDs must ma" + "tch the DoFs of the Config block pointed by this SetLowLevelPID block.\n\nParameters:\n- WBTPIDConfig Object Name" + ": The name of the object which store the PIDs. It must be loaded in the workspace before the simulation runs\n- P" + "ID Type: the type of the target PIDs to set" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend\n\n% SetLowLevelPIDs Initialization\n% ==============================\n\ntry\n " + " PIDObjectName = WBTPIDConfigObjectName;\n \n % Remove the apices if present\n if (PIDObjectName(1) == \"" + "'\" && ...\n PIDObjectName(end) == \"'\")\n PIDObjectName = PIDObjectName(2:end-1);\n end\n " + "\n % Get the struct from the class\n PIDs = evalin('base', PIDObjectName);\n if (isa(PIDs,'WBToolbox.WBT" + "PIDConfig'))\n WBTPIDConfigStruct = PIDs.getSimulinkParameters;\n end\nend\n" + Display "pidType = get_param(gcb,'pidType');\ndescription = sprintf('Set Low Level\\n%s PIDs', pidType);\ndi" + "sp(description);\n" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 124 + Type "edit" + Name "WBTPIDConfigObjectName" + Prompt "WBTPIDConfig Object Name" + Value "''" + Tunable "off" + } + Object { + $ObjectID 125 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Position" + Cell "Torque" + PropName "TypeOptions" + } + Name "pidType" + Prompt "PID Type" + Value "Position" + Evaluate "off" + Callback "% Get the mask parameter values. This is a cell\n% array of strings.\nmaskStr = get_param(gcb,'pidType'" + ");\n\nswitch maskStr\n case 'Position'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]')" + ";\n case 'Position Direct'\n set_param(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\n case " + "'Velocity'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n case 'Torque'\n s" + "et_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\n otherwise\n error('PID Type not recogn" + "ized');\nend\n\nclear maskStr" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 126 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 127 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 128 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 2 + Object { + $ObjectID 129 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "WBTPIDConfigObjectName" + } + Object { + $ObjectID 130 + $ClassName "Simulink.dialog.parameter.Popup" + Name "pidType" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + Object { + $ObjectID 131 + Object { + $PropName "DialogControls" + $ObjectID 132 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + } + System { + Name "Set LowLevel PIDs" + Location [114, 82, 1709, 948] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "600" + Block { + BlockType S-Function + Name "S-Function" + SID "1753" + Ports [] + Position [125, 39, 185, 71] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'SetLowLevelPID',WBTConfigParameters,configBlockAbsName,WBTPIDConfigStruct,pidType" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + } + } + Block { + BlockType SubSystem + Name "Set References" + SID "1767" + Ports [1] + Position [645, 230, 720, 280] + ZOrder 43 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 133 + $ClassName "Simulink.Mask" + Type "Set References" + Description "This block sets the references for the robot actuators.\nThe type of control mode is specified " + "as a parameter in the block.\n\nAssuming DoFs is the number of degrees of freedom configured in the Config block " + "associated:\n\nInput:\n- References: Vector of size DoFs, representing the references to be sent to the robot con" + "trolled joints.\n- Reference Speed (Position): Set the reference speed used by the trajectory generator of the IP" + "ositionControl interface\n\nParameters:\n- Control Mode: The control mode. Choose one of the supplied mode." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend" + Display "disp(get_param(gcb,'controlType'))\n%port_label('input',1,'References')" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 134 + Type "popup" + Array { + Type "Cell" + Dimension 7 + Cell "Position" + Cell "Position Direct" + Cell "Velocity" + Cell "Torque" + Cell "Open Loop" + Cell "PWM" + Cell "Current" + PropName "TypeOptions" + } + Name "controlType" + Prompt "Control Mode" + Value "Position" + Evaluate "off" + Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(gcb,'contro" + "lType');\n\nswitch maskStr\n case 'Position'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, " + "1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'on'});\n case 'Position Direct'\n set_param(gcb," + "'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off'});\n " + "case 'Velocity'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n set_param(gcb,'" + "MaskVisibilities', {'on';'off'});\n case 'Torque'\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6" + "039, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off'});\n case 'Open Loop'\n set_param(gcb," + "'BackgroundColor', '[0.98, 0.98, 0.98, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off'});\n case '" + "PWM'\n set_param(gcb,'BackgroundColor', '[1, 1, 1, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'" + "off'});\n case 'Current'\n set_param(gcb,'BackgroundColor', '[0.96, 0.77, 0.46, 1.0]');\n set_param" + "(gcb,'MaskVisibilities', {'on';'off'});\n otherwise\n error('Control Type not recognized.')\nend\n\nclear " + " maskStr" + } + Object { + $ObjectID 135 + Type "edit" + Name "refSpeed" + Prompt "Reference Speed" + Value "50" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 136 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 137 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 138 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 2 + Object { + $ObjectID 139 + $ClassName "Simulink.dialog.parameter.Popup" + Name "controlType" + } + Object { + $ObjectID 140 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "refSpeed" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + Object { + $ObjectID 141 + Object { + $PropName "DialogControls" + $ObjectID 142 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + } + System { + Name "Set References" + Location [2245, 90, 3840, 956] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "521" + Block { + BlockType Inport + Name "References" + SID "1768" + Position [55, 48, 85, 62] + ZOrder 24 + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1769" + Ports [1] + Position [125, 39, 185, 71] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'SetReferences',WBTConfigParameters,configBlockAbsName,controlType,refSpeed" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Line { + ZOrder 1 + SrcBlock "References" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + } + } + } + } + Block { + BlockType SubSystem + Name "wholeBodyModel" + SID "209" + Ports [] + Position [133, 16, 231, 113] + ZOrder -3 + ForegroundColor "white" + DropShadow on + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 143 + $ClassName "Simulink.Mask" + Display "image(imread('wholeBodyModel.png'),'center')" + } + System { + Name "wholeBodyModel" + Location [2245, 90, 3840, 956] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "381" + Block { + BlockType SubSystem + Name "Dynamics" + SID "369" + Ports [] + Position [364, 21, 471, 128] + ZOrder -1 + DropShadow on + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 144 + $ClassName "Simulink.Mask" + Display "image(imread('Dynamics.png'))" + } + System { + Name "Dynamics" + Location [2086, 89, 3681, 955] + Open on + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "269" + Block { + BlockType SubSystem + Name "Centroidal Momentum" + SID "1694" + Ports [4, 1] + Position [495, 101, 680, 164] + ZOrder 72 + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 145 + $ClassName "Simulink.Mask" + Type "Centroidal Momentum" + Description "This block computed the 6 element centroidal momentum, as defined in:\n\"Centroidal dynamics of a huma" + "noid robot\" - DE Orin, A Goswami, SH Lee - \nAutonomous Robots 35 (2-3), 161-176\n\nAssuming DoFs is the number of " + "degrees of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation betwe" + "en the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration o" + "f the joints.\n- Base velocity: Vector of size 6 representing the linear and angular velocity of the base frame.\n- " + "Joints velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n- Centroidal Momentum: 6-" + "element vector containg the centroidal momentum (3 value for linear momentum and 3 for angular momentum)" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 146 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 147 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 148 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 149 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "DescGroupVar" + } + } + System { + Name "Centroidal Momentum" + Location [300, 122, 1706, 880] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "347" + Block { + BlockType Inport + Name "Base Pose" + SID "1695" + Position [20, 18, 50, 32] + ZOrder 22 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joint configuration" + SID "1696" + Position [20, 53, 50, 67] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Base velocity" + SID "1697" + Position [20, 88, 50, 102] + ZOrder 26 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joints velocity" + SID "1698" + Position [20, 123, 50, 137] + ZOrder 27 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1699" + Ports [4, 1] + Position [180, 11, 255, 144] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'CentroidalMomentum',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Centroidal Momentum" + SID "1700" + Position [315, 73, 345, 87] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Centroidal Momentum" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Base Pose" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Joint configuration" + SrcPort 1 + DstBlock "S-Function" + DstPort 2 + } + Line { + ZOrder 4 + SrcBlock "Base velocity" + SrcPort 1 + DstBlock "S-Function" + DstPort 3 + } + Line { + ZOrder 5 + SrcBlock "Joints velocity" + SrcPort 1 + DstBlock "S-Function" + DstPort 4 + } + } + } + Block { + BlockType SubSystem + Name "Get Bias Forces" + SID "1724" + Ports [4, 1] + Position [400, 202, 540, 303] + ZOrder 73 + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 150 + $ClassName "Simulink.Mask" + Type "Get Generalized Bias Forces" + Description "This block retrieves the generalied bias forces of the robot.\n\nAssuming DoFs is the number of degree" + "s of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the" + " base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the " + "joints.\n- Base velocity: Vector of size 6 representing the linear and angular velocity of the base frame.\n- Joints" + " velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n- Bias Forces: a Dofs + 6 vecto" + "r representing the generalized bias forces of the robot." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 151 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 152 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 153 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 154 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "DescGroupVar" + } + } + System { + Name "Get Bias Forces" + Location [215, 73, 1810, 939] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "318" + Block { + BlockType Inport + Name "Base Pose" + SID "1776" + Position [10, 38, 40, 52] + ZOrder 22 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joint configuration" + SID "1777" + Position [10, 68, 40, 82] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Base velocity" + SID "1778" + Position [10, 98, 40, 112] + ZOrder 26 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joints velocity" + SID "1779" + Position [10, 128, 40, 142] + ZOrder 27 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant" + SID "1780" + Position [-5, 157, 50, 173] + ZOrder 30 + ShowName off + Value "zeros(6,1)" + } + Block { + BlockType Gain + Name "Gain" + SID "1781" + Position [110, 180, 140, 210] + ZOrder 31 + ShowName off + Gain "0" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType S-Function + Name "S-Function" + SID "1782" + Ports [6, 1] + Position [180, 24, 240, 216] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'InverseDynamics',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Bias Forces" + SID "1783" + Position [300, 113, 330, 127] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Bias Forces" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Base Pose" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Joint configuration" + SrcPort 1 + DstBlock "S-Function" + DstPort 2 + } + Line { + ZOrder 4 + SrcBlock "Base velocity" + SrcPort 1 + DstBlock "S-Function" + DstPort 3 + } + Line { + ZOrder 5 + SrcBlock "Joints velocity" + SrcPort 1 + Points [44, 0] + Branch { + ZOrder 11 + Points [0, 60] + DstBlock "Gain" + DstPort 1 + } + Branch { + ZOrder 10 + DstBlock "S-Function" + DstPort 4 + } + } + Line { + ZOrder 8 + SrcBlock "Constant" + SrcPort 1 + DstBlock "S-Function" + DstPort 5 + } + Line { + ZOrder 9 + SrcBlock "Gain" + SrcPort 1 + DstBlock "S-Function" + DstPort 6 + } + } + } + Block { + BlockType SubSystem + Name "Get Gravity Forces" + SID "1733" + Ports [2, 1] + Position [605, 200, 745, 300] + ZOrder 74 + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 155 + $ClassName "Simulink.Mask" + Type "Gravity bias" + Description "This block compute the generalized bias forces due to the gravity\n\nAssuming DoFs is the number of de" + "grees of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between" + " the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of " + "the joints.\n\nOutput:\n- Gravity: a Dofs + 6 vector representing the torques due to the gravity." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "off" + Object { + $PropName "DialogControls" + $ObjectID 156 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 157 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 158 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 159 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "DescGroupVar" + } + } + System { + Name "Get Gravity Forces" + Location [300, 122, 1706, 880] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "285" + Block { + BlockType Inport + Name "Base Pose" + SID "1784" + Position [10, 38, 40, 52] + ZOrder 22 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joint configuration" + SID "1785" + Position [10, 68, 40, 82] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Constant + Name "Constant" + SID "1786" + Position [0, 97, 55, 113] + ZOrder 32 + ShowName off + Value "zeros(6,1)" + } + Block { + BlockType Gain + Name "Gain" + SID "1787" + Position [100, 120, 130, 150] + ZOrder 33 + ShowName off + Gain "0" + ParamDataTypeStr "Inherit: Inherit via internal rule" + OutDataTypeStr "Inherit: Inherit via internal rule" + SaturateOnIntegerOverflow off + } + Block { + BlockType S-Function + Name "S-Function" + SID "1788" + Ports [6, 1] + Position [180, 24, 240, 216] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'InverseDynamics',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Gravity Torques" + SID "1789" + Position [300, 113, 330, 127] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Gravity Torques" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Base Pose" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Joint configuration" + SrcPort 1 + Points [41, 0] + Branch { + ZOrder 12 + Points [0, 60] + DstBlock "Gain" + DstPort 1 + } + Branch { + ZOrder 11 + DstBlock "S-Function" + DstPort 2 + } + } + Line { + ZOrder 8 + SrcBlock "Constant" + SrcPort 1 + Points [4, 0] + Branch { + ZOrder 10 + Points [0, 60] + DstBlock "S-Function" + DstPort 5 + } + Branch { + ZOrder 9 + DstBlock "S-Function" + DstPort 3 + } + } + Line { + ZOrder 13 + SrcBlock "Gain" + SrcPort 1 + Points [13, 0] + Branch { + ZOrder 15 + Points [0, 60] + DstBlock "S-Function" + DstPort 6 + } + Branch { + ZOrder 14 + DstBlock "S-Function" + DstPort 4 + } + } + } + } + Block { + BlockType S-Function + Name "Inverse Dynamics" + SID "1748" + Ports [6, 1] + Position [190, 199, 355, 331] + ZOrder 75 + FunctionName "WBToolbox" + Parameters "'InverseDynamics',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 160 + $ClassName "Simulink.Mask" + Type "Inverse Dynamics" + Description "This block compute the inverse dynamics of the robot.\n\nAssuming DoFs is the number of degrees of fre" + "edom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the base fr" + "ame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the joints.\n" + "- Base velocity: Vector of size 6 representing the linear and angular velocity of the base frame.\n- Joints velocity" + ": Vector of size DoFs, representing the velocity of the joints.\n- Base acceleration: Vector of size 6 representing " + "the linear and angular acceleration of the base frame.\n- Joints acceleration: Vector of size DoFs, representing the" + " acceleration of the joints.\n\nOutput:\n- Torques: a Dofs + 6 vector representing the corresponding torques require" + "d to achive the desired accelerations given the robot state" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + Display "port_label('output', 1, 'Torques')\n\nport_label('input', 1, 'Base pose')\nport_label('input', 2, 'Joints" + " configuration')\nport_label('input', 3, 'Base velocity')\nport_label('input', 4, 'Joints velocity')\nport_label('in" + "put', 5, 'Base acceleration')\nport_label('input', 6, 'Joints acceleration')\n" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 161 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 162 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 163 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 164 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "DescGroupVar" + } + } + } + Block { + BlockType SubSystem + Name "Mass Matrix" + SID "1633" + Ports [2, 1] + Position [250, 104, 390, 171] + ZOrder 32 + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 165 + $ClassName "Simulink.Mask" + Type "Mass Matrix" + Description "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of degrees of freedom of th" + "e robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the base frame and the " + "world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the joints.\n\nOutput:\n" + "- Mass Matrix: a (Dofs + 6) x (Dofs + 6) matrix representing the mass matrix of the robot" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + RunInitForIconRedraw "on" + Object { + $PropName "DialogControls" + $ObjectID 166 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 167 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 168 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 169 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "DescGroupVar" + } + } + System { + Name "Mass Matrix" + Location [300, 122, 1706, 880] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "455" + Block { + BlockType Inport + Name "Base Pose" + SID "1634" + Position [20, 23, 50, 37] + ZOrder 22 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joint configuration" + SID "1635" + Position [20, 63, 50, 77] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1636" + Ports [2, 1] + Position [125, 37, 185, 68] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'MassMatrix',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Mass Matrix" + SID "1637" + Position [245, 48, 275, 62] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Mass Matrix" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Base Pose" + SrcPort 1 + Points [36, 0; 0, 15] + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Joint configuration" + SrcPort 1 + Points [33, 0; 0, -10] + DstBlock "S-Function" + DstPort 2 + } + } + } + } + } + Block { + BlockType SubSystem + Name "Jacobians" + SID "202" + Ports [] + Position [217, 20, 324, 127] + ZOrder -3 + DropShadow on + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 170 + $ClassName "Simulink.Mask" + Display "image(imread('jacobian.png'))" + } + System { + Name "Jacobians" + Location [325, 44, 1920, 910] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "397" + Block { + BlockType SubSystem + Name "DotJ Nu" + SID "1683" + Ports [4, 1] + Position [590, 170, 755, 265] + ZOrder 67 + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 171 + $ClassName "Simulink.Mask" + Type "DotJ Nu" + Description "This block computes the product between the time derivative of the Jacobian of the specified frame and" + " the state (base and joints) velocity vector.\n\nAssuming DoFs is the number of degrees of freedom of the robot:\n\n" + "Input:\n- Base pose: 4x4 matrix representing the homogenous transformation between the the base frame and the world " + "frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the joints.\n- Base velocity: " + "Vector of size 6 representing the linear and angular velocity of the base frame.\n- Joints velocity: Vector of size " + "DoFs, representing the velocity of the joints.\n\nOutput:\n- dotJ nu: a 6-d vector representing the product between " + "the time derivative of the\n Jacobian of the specified frame and the state velocity vector\n\nParameters:\n-" + " Frame name: the name of the frame. It should be specified in the URDF model." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + Display "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('dot J_{',escapedFrameN" + "ame,'} \\nu'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('input'," + " 2, 'Joint configuration')\nport_label('input', 3, 'Base velocity')\nport_label('input', 4, 'Joint velocity')\n\ncle" + "ar escapedFrameName;" + RunInitForIconRedraw "on" + Object { + $PropName "Parameters" + $ObjectID 172 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 173 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 174 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 175 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 176 + $ClassName "Simulink.dialog.parameter.Edit" + Name "frameName" + } + Object { + $ObjectID 177 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 178 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + System { + Name "DotJ Nu" + Location [300, 122, 1706, 880] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "428" + Block { + BlockType Inport + Name "Base Pose" + SID "1684" + Position [20, 13, 50, 27] + ZOrder 22 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joint configuration" + SID "1685" + Position [20, 43, 50, 57] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Base velocity" + SID "1686" + Position [20, 73, 50, 87] + ZOrder 26 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joints velocity" + SID "1687" + Position [20, 103, 50, 117] + ZOrder 27 + Port "4" + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1688" + Ports [4, 1] + Position [125, 4, 190, 126] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'DotJNu',WBTConfigParameters,configBlockAbsName,frameName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "dotJ dotnu" + SID "1689" + Position [245, 58, 275, 72] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "Joint configuration" + SrcPort 1 + DstBlock "S-Function" + DstPort 2 + } + Line { + ZOrder 2 + SrcBlock "Base Pose" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "dotJ dotnu" + DstPort 1 + } + Line { + ZOrder 4 + SrcBlock "Base velocity" + SrcPort 1 + DstBlock "S-Function" + DstPort 3 + } + Line { + ZOrder 5 + SrcBlock "Joints velocity" + SrcPort 1 + DstBlock "S-Function" + DstPort 4 + } + } + } + Block { + BlockType SubSystem + Name "Jacobian" + SID "1663" + Ports [2, 1] + Position [380, 190, 540, 245] + ZOrder 39 + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 179 + $ClassName "Simulink.Mask" + Type "Jacobian" + Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of degrees of" + " freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between the the" + " base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configuration of the " + "joints.\n\nOutput:\n- Jacobian: a 6x6+dofs matrix representing the Jacobian of the specified frame written in the wo" + "rld frame.\n\nParameters:\n- Frame name: the name of the frame. It should be specified in the URDF model.\n" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + Display "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('{}^{world} J_{',escape" + "dFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('input" + "', 2, 'Joint configuration')\n\nclear escapedFrameName;" + RunInitForIconRedraw "on" + Object { + $PropName "Parameters" + $ObjectID 180 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 181 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 182 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 183 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 184 + $ClassName "Simulink.dialog.parameter.Edit" + Name "frameName" + } + Object { + $ObjectID 185 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 186 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + System { + Name "Jacobian" + Location [300, 122, 1706, 880] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "430" + Block { + BlockType Inport + Name "Base Pose" + SID "1664" + Position [20, 23, 50, 37] + ZOrder 22 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joint configuration" + SID "1665" + Position [20, 63, 50, 77] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1666" + Ports [2, 1] + Position [125, 37, 185, 68] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'Jacobian',WBTConfigParameters,configBlockAbsName,frameName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Forward Kinematics" + SID "1667" + Position [245, 48, 275, 62] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Forward Kinematics" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Base Pose" + SrcPort 1 + Points [36, 0; 0, 15] + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Joint configuration" + SrcPort 1 + Points [33, 0; 0, -10] + DstBlock "S-Function" + DstPort 2 + } + } + } + } + } + Block { + BlockType SubSystem + Name "Kinematics" + SID "176" + Ports [] + Position [70, 20, 177, 127] + ZOrder -17 + DropShadow on + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 187 + $ClassName "Simulink.Mask" + Display "image(imread('forwardKinematics.png'))" + } + System { + Name "Kinematics" + Location [325, 44, 1920, 910] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "338" + Block { + BlockType SubSystem + Name "Forward Kinematics" + SID "1647" + Ports [2, 1] + Position [350, 103, 500, 167] + ZOrder 34 + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 188 + $ClassName "Simulink.Mask" + Type "Forward Kinematics" + Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " + "degrees of freedom of the robot:\n\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation betwe" + "en the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the configurati" + "on of the joints.\n\nOutput:\n- Forward Kinematics: a 4x4 matrix representing the homogenous transformation between " + "the specified frame and the world frame.\n\nParameters:\n- Frame name: the name of the frame. It should be specified" + " in the URDF model." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig] = WB" + "Toolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n clear " + "WBTConfig;\ncatch\nend" + Display "escapedFrameName = strrep(frameName, '_', '\\_');\nport_label('output', 1, strcat('{}^{world} H_{',escape" + "dFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('input" + "', 2, 'Joint configuration')\n\nclear escapedFrameName;" + Object { + $PropName "Parameters" + $ObjectID 189 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 190 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 191 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 192 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 193 + $ClassName "Simulink.dialog.parameter.Edit" + Name "frameName" + } + Object { + $ObjectID 194 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 195 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" + "igBlock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system" + "(configBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block n" + "ame: %s',gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + System { + Name "Forward Kinematics" + Location [300, 122, 1706, 880] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "430" + Block { + BlockType Inport + Name "Base Pose" + SID "1648" + Position [20, 23, 50, 37] + ZOrder 22 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Joint configuration" + SID "1649" + Position [20, 63, 50, 77] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1650" + Ports [2, 1] + Position [125, 37, 185, 68] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'ForwardKinematics',WBTConfigParameters,configBlockAbsName,frameName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Forward Kinematics" + SID "1651" + Position [245, 48, 275, 62] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Forward Kinematics" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Base Pose" + SrcPort 1 + Points [36, 0; 0, 15] + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Joint configuration" + SrcPort 1 + Points [33, 0; 0, -10] + DstBlock "S-Function" + DstPort 2 + } + } + } + Block { + BlockType SubSystem + Name "Inverse Kinematics" + SID "1754" + Ports [3, 2] + Position [350, 198, 525, 262] + ZOrder 35 + Commented "on" + InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" + ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" + "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" + "nd" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 196 + $ClassName "Simulink.Mask" + Type "Forward Kinematics" + Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " + "degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between" + "\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the" + " configuration \n of the joints.\n\nOutput:\n- Forward Kinematics: a 4x4 matrix representing t" + "he homogenous transformation between\n the specified frame and the world frame.\n\nParameters:\n- Frame " + "name: the name of the frame. It should be specified in the URDF model.\n\n- Robot Port Name: Name of the ports opene" + "d by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the \n " + " Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underlyin" + "g Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interface\n" + "- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" + Display "escapedBaseFrameName = strrep(baseFrame, '_', '\\_');\nescapedEndEffFrameName = strrep(endEffFrame, '_', " + "'\\_');\n\n%port_label('output', 1, strcat('{}^{world} H_{',escapedFrameName,'}'), 'texmode','on')\n\n%port_label('i" + "nput', 1, '{}^{world} H_{base}', 'texmode','on')\n%port_label('input', 2, 'Joint configuration')\n\n%clear escapedFr" + "ameName;\n\n\n% if strcmp(robotPart, 'left')\n% prefix = 'l';\n% else\n% prefix = 'r';\n% end\n% \nport_labe" + "l('input', 1, strcat('{}^{',escapedBaseFrameName,'} H_{',escapedEndEffFrameName,'}^d'), 'texmode','on');\n\nport_lab" + "el('input', 2, '{}^{world} H_{base}', 'texmode','on')\nport_label('input', 3, 'q_j', 'texmode','on');\n% \n% \nport_" + "label('output', 1, '{}^{world} H_{base}^d', 'texmode','on')\nport_label('output', 2, 'q_j^d', 'texmode','on');\n\n" + Array { + Type "Simulink.MaskParameter" + Dimension 7 + Object { + $ObjectID 197 + Type "edit" + Name "baseFrame" + Prompt "Base Frame" + Value "'root_link'" + } + Object { + $ObjectID 198 + Type "edit" + Name "endEffFrame" + Prompt "End Effector frame" + Value "'l_sole'" + } + Object { + $ObjectID 199 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Full Constraint (Position and Orientation)" + Cell "Position only constraint" + PropName "TypeOptions" + } + Name "optOption" + Prompt "Optimization Option" + Value "Full Constraint (Position and Orientation)" + } + Object { + $ObjectID 200 + Type "edit" + Name "robotName" + Prompt "Robot Port Name" + Value "WBT_robotName" + Tunable "off" + } + Object { + $ObjectID 201 + Type "edit" + Name "localName" + Prompt "Model Name" + Value "WBT_modelName" + Tunable "off" + } + Object { + $ObjectID 202 + Type "edit" + Name "wbiFile" + Prompt "WBI filename" + Value "WBT_wbiFilename" + Tunable "off" + } + Object { + $ObjectID 203 + Type "edit" + Name "wbiList" + Prompt "WBI list name" + Value "WBT_wbiList" + Tunable "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 204 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 205 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 206 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 207 + $ClassName "Simulink.dialog.TabContainer" + Array { + Type "Simulink.dialog.Tab" + Dimension 2 + Object { + $ObjectID 208 + Prompt "Block parameters" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 209 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "baseFrame" + } + Object { + $ObjectID 210 + $ClassName "Simulink.dialog.parameter.Edit" + Name "endEffFrame" + } + Object { + $ObjectID 211 + $ClassName "Simulink.dialog.parameter.Popup" + Name "optOption" + } + PropName "DialogControls" + } + Name "Container8" + } + Object { + $ObjectID 212 + Prompt "WBI parameters" + Array { + Type "Simulink.dialog.parameter.Edit" + Dimension 4 + Object { + $ObjectID 213 + Name "robotName" + } + Object { + $ObjectID 214 + Name "localName" + } + Object { + $ObjectID 215 + Name "wbiFile" + } + Object { + $ObjectID 216 + Name "wbiList" + } + PropName "DialogControls" + } + Name "Container5" + } + PropName "DialogControls" + } + Name "Container4" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + System { + Name "Inverse Kinematics" + Location [0, 23, 1280, 744] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "312" + Block { + BlockType Inport + Name "Desired frame pose" + SID "1759" + Position [10, 13, 40, 27] + ZOrder 26 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Current Base Pose" + SID "1755" + Position [10, 43, 40, 57] + ZOrder 22 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Current Joint configuration" + SID "1756" + Position [10, 73, 40, 87] + ZOrder 24 + Port "3" + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1757" + Ports [3, 2] + Position [145, 4, 225, 96] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'InverseKinematics',robotName,localName,wbiFile,wbiList,baseFrame, endEffFrame,optOption" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Desired Base Pose" + SID "1758" + Position [280, 23, 310, 37] + ZOrder 25 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "Desired Joint Configuration" + SID "1760" + Position [280, 68, 310, 82] + ZOrder 27 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Desired Base Pose" + DstPort 1 + } + Line { + ZOrder 5 + SrcBlock "Current Base Pose" + SrcPort 1 + DstBlock "S-Function" + DstPort 2 + } + Line { + ZOrder 4 + SrcBlock "Current Joint configuration" + SrcPort 1 + DstBlock "S-Function" + DstPort 3 + } + Line { + ZOrder 7 + SrcBlock "Desired frame pose" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 8 + SrcBlock "S-Function" + SrcPort 2 + DstBlock "Desired Joint Configuration" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Remote Inverse Kinematics" + SID "1761" + Ports [2, 1] + Position [560, 105, 720, 165] + ZOrder 66 + Commented "on" + InitFcn "if ~exist('WBT_robotName','var')\n WBT_robotName = '';\nend\nif ~exist('WBT_modelName','var'" + ")\n WBT_modelName = 'WBT_simulink';\nend\nif ~exist('WBT_wbiFilename','var')\n WBT_wbiFilename = 'yarpWho" + "leBodyInterface.ini';\nend\nif ~exist('WBT_wbiList','var')\n WBT_wbiList = 'ROBOT_TORQUE_CONTROL_JOINTS';\ne" + "nd" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 217 + $ClassName "Simulink.Mask" + Type "Forward Kinematics" + Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " + "degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformation between" + "\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, representing the" + " configuration \n of the joints.\n\nOutput:\n- Forward Kinematics: a 4x4 matrix representing t" + "he homogenous transformation between\n the specified frame and the world frame.\n\nParameters:\n- Frame " + "name: the name of the frame. It should be specified in the URDF model.\n\n- Robot Port Name: Name of the ports opene" + "d by the robot. (e.g. icub).\n Set an empty string ('') to use the name specified in the \n " + " Whole Body Interface configuration file.\n- Model Name: Prefix name of the ports opened by the underlyin" + "g Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Interface\n" + "- WBI List Name: name of the list of joints used to configure the Whole Body Interface\n" + Display "disp(solverName)\n\n\n\n% escapedBaseFrameName = strrep(baseFrame, '_', '\\_');\n% escapedEndEffFrameName" + " = strrep(endEffFrame, '_', '\\_');\n% \n% %port_label('output', 1, strcat('{}^{world} H_{',escapedFrameName,'}'), '" + "texmode','on')\n% \n% %port_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\n% %port_label('input', 2, 'Joi" + "nt configuration')\n% \n% %clear escapedFrameName;\n% \n% \n% % if strcmp(robotPart, 'left')\n% % prefix = 'l';\n" + "% % else\n% % prefix = 'r';\n% % end\n% % \n\nport_label('input', 1, 'H^d', 'texmode','on');\n% port_label('inpu" + "t', 1, strcat('{}^{',escapedBaseFrameName,'} H_{',escapedEndEffFrameName,'}^d'), 'texmode','on');\n% \n% port_label(" + "'input', 2, '{}^{world} H_{base}', 'texmode','on')\nport_label('input', 2, 'q_j(0)', 'texmode','on');\n% % \n% % \n%" + " port_label('output', 1, '{}^{world} H_{base}^d', 'texmode','on')\nport_label('output', 1, 'q_j^d', 'texmode','on');" + "\n\n" + Array { + Type "Simulink.MaskParameter" + Dimension 3 + Object { + $ObjectID 218 + Type "edit" + Name "solverName" + Prompt "Solver Name" + Value "'/cartesianSolver'" + } + Object { + $ObjectID 219 + Type "edit" + Name "dofs" + Prompt "#Dofs" + Value "12" + Tunable "off" + } + Object { + $ObjectID 220 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Full Constraint (Position and Orientation)" + Cell "Position only constraint" + PropName "TypeOptions" + } + Name "optOption" + Prompt "Optimization Option" + Value "Full Constraint (Position and Orientation)" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 221 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 222 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 223 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 224 + $ClassName "Simulink.dialog.TabContainer" + Object { + $PropName "DialogControls" + $ObjectID 225 + $ClassName "Simulink.dialog.Tab" + Prompt "Block parameters" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 226 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "solverName" + } + Object { + $ObjectID 227 + $ClassName "Simulink.dialog.parameter.Edit" + Name "dofs" + } + Object { + $ObjectID 228 + $ClassName "Simulink.dialog.parameter.Popup" + Name "optOption" + } + PropName "DialogControls" + } + Name "Container8" + } + Name "Container4" + } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + System { + Name "Remote Inverse Kinematics" + Location [853, 51, 2214, 1013] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "375" + Block { + BlockType Inport + Name "Desired frame pose" + SID "1762" + Position [10, 23, 40, 37] + ZOrder 26 + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Current Joint configuration" + SID "1763" + Position [10, 68, 40, 82] + ZOrder 24 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType S-Function + Name "S-Function" + SID "1764" + Ports [2, 1] + Position [145, 6, 225, 99] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'RemoteInverseKinematics',solverName, dofs, optOption" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Desired Joint Configuration" + SID "1765" + Position [285, 48, 315, 62] + ZOrder 27 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Desired Joint Configuration" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Desired frame pose" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + Line { + ZOrder 3 + SrcBlock "Current Joint configuration" + SrcPort 1 + DstBlock "S-Function" + DstPort 2 + } + } + } + } + } + } + } + Block { + BlockType SubSystem + Name "wholeBodyStates" + SID "206" + Ports [] + Position [16, 17, 114, 114] + ZOrder -4 + ForegroundColor "white" + DropShadow on + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 229 + $ClassName "Simulink.Mask" + Display "image(imread('wholeBodyStates.png'),'center');" + } + System { + Name "wholeBodyStates" + Location [2245, 90, 3840, 956] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "600" + Block { + BlockType SubSystem + Name "Get Limits" + SID "1690" + Ports [0, 2] + Position [395, 218, 515, 247] + ZOrder 68 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 230 + $ClassName "Simulink.Mask" + Type "Get Limits" + Description "This block provides the joint limits gathering data from either the Robot's Control Board or UR" + "DF model.\n\nOutput:\n\n* Max: 1xDoFs vector containing the maximum limit\n* Min: 1xDoFs vector containing the m" + "aximum limit\n\nMeasures are in radians." + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend\n\n% GetLimits Initialization\n% =========================\n\nsourceLimit = get_p" + "aram(gcb,'limitsSource');\ntypeLimit = get_param(gcb,'limitsType');\nnotifyError = false;\n\nswitch sourceLimit\n" + " case 'ControlBoard'\n switch typeLimit\n case 'Position'\n limitsType = 'Con" + "trolBoardPosition';\n case 'Velocity'\n limitsType = 'ControlBoardVelocity';\n " + " otherwise\n notifyError = true;\n end\n case 'URDF'\n switch typeLimit\n " + " case 'Position'\n limitsType = 'ModelPosition';\n otherwise\n noti" + "fyError = true;\n end\nend\n\nif notifyError\n error('Limit Type not recognized');\nend\n\nclear source" + "Limit typeLimit notifyError" + SelfModifiable "on" + Display "fprintf('%s\\n(%s)',get_param(gcb,'limitsType'),get_param(gcb,'limitsSource'))\n\nport_label('outpu" + "t',1,'Min')\nport_label('output',2,'Max')" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 231 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "ControlBoard" + Cell "URDF" + PropName "TypeOptions" + } + Name "limitsSource" + Prompt "Limits Source" + Value "ControlBoard" + Callback "sourceLimit = get_param(gcb,'limitsSource');\nblockParameters = Simulink.Mask.get(gcb).Parameters;\nlimit" + "sTypeBlockParam = blockParameters(2);\n\nswitch sourceLimit\n case 'ControlBoard'\n limitsTypeBlockParam.T" + "ypeOptions = {'Position','Velocity'};\n %limitsTypeBlockParam.Value = limitsTypeBlockParam.TypeOptions{1};\n " + " case 'URDF'\n limitsTypeBlockParam.TypeOptions = {'Position'};\n %limitsTypeBlockParam.Value = limi" + "tsTypeBlockParam.TypeOptions{1};\n otherwise\n error('Limit Source not recognized');\nend\n\nclear sourceL" + "imit blockParameters limitsTypeBlockParam" + } + Object { + $ObjectID 232 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Position" + Cell "Velocity" + PropName "TypeOptions" + } + Name "limitsType" + Prompt "Limits Type" + Value "Position" + Evaluate "off" + Callback "% Get the mask parameter values. This is a cell array of strings.\n% \nmaskStr = get_param(gcb,'limitsT" + "ype');\nif strcmp(maskStr, 'Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]');\nelse" + "if strcmp(maskStr, 'Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\nelseif strcm" + "p(maskStr, 'Acceleration')\n set_param(gcb,'BackgroundColor', '[0.9255, 0.8706, 0.1333, 1.0]');\nelseif strcmp(ma" + "skStr, 'Torque')\n set_param(gcb,'BackgroundColor', '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear maskStr" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 233 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 234 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 235 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.parameter.Popup" + Dimension 2 + Object { + $ObjectID 236 + Name "limitsSource" + } + Object { + $ObjectID 237 + Name "limitsType" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" + } + Object { + $ObjectID 238 + Object { + $PropName "DialogControls" + $ObjectID 239 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + } + System { + Name "Get Limits" + Location [300, 122, 1706, 880] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "692" + SIDHighWatermark "1773" + Block { + BlockType S-Function + Name "S-Function" + SID "1690:1691" + Ports [0, 2] + Position [115, 33, 175, 102] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'GetLimits',WBTConfigParameters,configBlockAbsName,limitsType" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Min" + SID "1690:1692" + Position [220, 43, 250, 57] + ZOrder 25 + IconDisplay "Port number" + } + Block { + BlockType Outport + Name "Max" + SID "1690:1693" + Position [220, 78, 250, 92] + ZOrder 26 + Port "2" + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Min" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "S-Function" + SrcPort 2 + DstBlock "Max" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "Get Measurement" + SID "1671" + Ports [0, 1] + Position [395, 156, 515, 184] + ZOrder 53 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + RequestExecContextInheritance off + Variant off + Object { + $PropName "MaskObject" + $ObjectID 240 + $ClassName "Simulink.Mask" + Type "Get Measurement" + Description "This block gathers the measurement of the specified type.\n\nAssuming DoFs is the number of deg" + "rees of freedom of the robot,\n\nOutput:\n* References: Vector of size DoFs, representing the requested measureme" + "nt.\n\nParameters:\n* Control Mode: The control mode. Choose one of the supplied mode.\n" + Initialization "% WBBlock Initialization\n% ======================\n\ntry\n [configBlockAbsName, WBTConfig" + "] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimulinkParameters;\n " + " clear WBTConfig;\ncatch\nend" + Display "port_label('output', 1, get_param(gcb,'measuredType'))" + RunInitForIconRedraw "off" + Object { + $PropName "Parameters" + $ObjectID 241 + $ClassName "Simulink.MaskParameter" + Type "popup" + Array { + Type "Cell" + Dimension 4 + Cell "Joints Position" + Cell "Joints Velocity" + Cell "Joints Acceleration" + Cell "Joints Torque" + PropName "TypeOptions" + } + Name "measuredType" + Prompt "Estimate Type" + Value "Joints Position" + Evaluate "off" + Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(" + "gcb,'measuredType');\nif strcmp(maskStr, 'Joints Position')\n set_param(gcb,'BackgroundColor', '[0.5137, 0.8" + "510, 0.6706, 1.0]');\nelseif strcmp(maskStr, 'Joints Velocity')\n set_param(gcb,'BackgroundColor', '[0.5137," + " 0.6745, 1.0000, 1.0]');\nelseif strcmp(maskStr, 'Joints Acceleration')\n set_param(gcb,'BackgroundColor', '" + "[0.9255, 0.8706, 0.1333, 1.0]');\nelseif strcmp(maskStr, 'Joints Torque')\n set_param(gcb,'BackgroundColor'," + " '[0.8275, 0.5765, 0.6039, 1.0]');\nend\nclear maskStr" + } + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 242 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 243 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 244 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 245 + $ClassName "Simulink.dialog.parameter.Popup" + Name "measuredType" + } + Name "ParameterGroupVar" + } + Object { + $ObjectID 246 + Object { + $PropName "DialogControls" + $ObjectID 247 + $ClassName "Simulink.dialog.Button" + Prompt "Toggle Config Block Highlighting" + Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(configBl" + "ock,'HiliteAncestors'),'none'))\n hilite_system(configBlock,'find');\n else\n hilite_system(confi" + "gBlock,'none');\n end\ncatch\n error('[%s:ShowConfigBlock:Callback] Failed to process config block name: %s'" + ",gcb,configBlock);\nend\n\nclear configBlock" + Name "toggleHighlighting" + } + Name "Container8" + } + PropName "DialogControls" + } + } + System { + Name "Get Measurement" + Location [109, 127, 1339, 823] + Open off + PortBlocksUseCompactNotation off + ModelBrowserVisibility off + 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 "600" + Block { + BlockType S-Function + Name "S-Function" + SID "1672" + Ports [0, 1] + Position [125, 39, 185, 71] + ZOrder 19 + FunctionName "WBToolbox" + Parameters "'GetMeasurement',WBTConfigParameters,configBlockAbsName,measuredType" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Estimate" + SID "1673" + Position [210, 48, 240, 62] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Estimate" + DstPort 1 + } + } + } + } + } + Annotation { + SID "1213" + Name "WHOLE BODY TOOLBOX" + Position [172, 149, 319, 165] + InternalMargins [0, 0, 0, 0] + FixedHeight off + FixedWidth off + ForegroundColor "white" + BackgroundColor "black" + ZOrder -1 + FontName "Sans Serif" + FontSize 12 + } + Annotation { + SID "1214" + Name "Copyright 2013-2017 iCub Facility Department, Istituto Italiano di Tecnologia\nThis software was s" + "upported by the FP7 EU Project CoDyCO (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b))\nhttp://www.c" + "odyco.eu" + Position [102, 184, 380, 207] + InternalMargins [0, 0, 0, 0] + FixedHeight off + FixedWidth off + ForegroundColor "white" + BackgroundColor "black" + ZOrder -2 + FontSize 5 + } + } +} +#Finite State Machines +# +# Stateflow 80000011 +# +# +Stateflow { + machine { + id 1 + name "WBToolboxLibrary_repository" + created "06-Feb-2014 11:51:26" + isLibrary 1 + sfVersion 80000011 + firstTarget 26 + } + chart { + id 2 + machine 1 + name "Utilities/holder\n/MATLAB Function" + windowPosition [1152 -205 -179 985] + viewLimits [0 156.75 0 153.75] + screen [1 1 3046 1050 1.25] + treeNode [0 3 0 0] + viewObj 2 + toolbarMode LIBRARY_TOOLBAR + ssIdHighWaterMark 9 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 1 + disableImplicitCasting 1 + eml { + name "fcn" + } + firstData 4 + firstTransition 8 + firstJunction 7 + } + state { + id 3 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 2 + treeNode [2 0 0 0] + superState SUBCHART + subviewer 2 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function s0 = fcn(s, ~)\npersistent state\n%#codegen\n\nif isempty(state)\n state = s;\nend\n\n" + "s0 = state;" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 4 + ssIdNumber 7 + name "s" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [2 0 5] + } + data { + id 5 + ssIdNumber 8 + name "s0" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [2 4 6] + } + data { + id 6 + ssIdNumber 9 + name "unused" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [2 5 0] + } + junction { + id 7 + position [23.5747 49.5747 7] + chart 2 + subviewer 2 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [2 0 0] + } + transition { + id 8 + labelString "{eML_blk_kernel();}" + labelPosition [76.125 85.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 7 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 2 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 2 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [2 0 0] + } + instance { + id 9 + machine 1 + name "Utilities/holder\n/MATLAB Function" + chart 2 + } + chart { + id 10 + machine 1 + name "Utilities/DampPinv/Damped Pseudo Inverse" + windowPosition [699 -205 167 985] + viewLimits [0 156.75 0 153.75] + screen [1 1 3046 1050 1.25] + treeNode [0 11 0 0] + viewObj 10 + toolbarMode LIBRARY_TOOLBAR + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 6 + disableImplicitCasting 1 + eml { + name "fcn" + } + firstData 12 + firstTransition 16 + firstJunction 15 + } + state { + id 11 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 10 + treeNode [10 0 0 0] + superState SUBCHART + subviewer 10 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function DPinv = fcn(mat,sigma)\n% Economody size svd of mat\n[U,S,V] = svd(mat,'econ');\n% Damp" + "ed version of S with sigma\nS(S>sigma)=S(S>sigma)./((S(S>sigma)).^2+sigma^2);\n% Damped pseudoinverse\nDPinv = V" + "*pinv(S)*U';" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 12 + ssIdNumber 4 + name "mat" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [10 0 13] + } + data { + id 13 + ssIdNumber 5 + name "DPinv" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [10 12 14] + } + data { + id 14 + ssIdNumber 8 + name "sigma" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [10 13 0] + } + junction { + id 15 + position [23.5747 49.5747 7] + chart 10 + subviewer 10 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [10 0 0] + } + transition { + id 16 + labelString "{eML_blk_kernel();}" + labelPosition [80.125 91.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 15 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 10 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 10 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [10 0 0] + } + instance { + id 17 + machine 1 + name "Utilities/DampPinv/Damped Pseudo Inverse" + chart 10 + } + chart { + id 18 + machine 1 + name "Utilities/TruncPinv/Truncated PseudoInverse" + windowPosition [649 -205 167 985] + viewLimits [0 156.75 0 153.75] + screen [1 1 3046 1050 1.25] + treeNode [0 19 0 0] + viewObj 18 + toolbarMode LIBRARY_TOOLBAR + ssIdHighWaterMark 8 + decomposition CLUSTER_CHART + type EML_CHART + chartFileNumber 7 + disableImplicitCasting 1 + eml { + name "fcn" + } + firstData 20 + firstTransition 24 + firstJunction 23 + } + state { + id 19 + labelString "eML_blk_kernel()" + position [18 64.5 118 66] + fontSize 12 + chart 18 + treeNode [18 0 0 0] + superState SUBCHART + subviewer 18 + ssIdNumber 1 + type FUNC_STATE + decomposition CLUSTER_STATE + eml { + isEML 1 + script "function TPinv = fcn(mat,tol)\n%#codegen\n[U,S,V] = svd(mat,'econ');\n% Setting to zero value\n%" + " Setting to 1/S(i,i) singular values greater than tol\n S(S>tol)=1./S(S>tol);\n TPinv = V*pinv(S)*U';" + editorLayout "100 M4x1[10 5 700 500]" + } + } + data { + id 20 + ssIdNumber 4 + name "mat" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [18 0 21] + } + data { + id 21 + ssIdNumber 5 + name "TPinv" + scope OUTPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_NO + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [18 20 22] + } + data { + id 22 + ssIdNumber 8 + name "tol" + scope INPUT_DATA + machine 1 + props { + array { + size "-1" + } + type { + method SF_INHERITED_TYPE + primitive SF_DOUBLE_TYPE + isSigned 1 + wordLength "16" + } + complexity SF_COMPLEX_INHERITED + frame SF_FRAME_INHERITED + unit { + name "inherit" + } + } + dataType "Inherit: Same as Simulink" + linkNode [18 21 0] + } + junction { + id 23 + position [23.5747 49.5747 7] + chart 18 + subviewer 18 + ssIdNumber 3 + type CONNECTIVE_JUNCTION + linkNode [18 0 0] + } + transition { + id 24 + labelString "{eML_blk_kernel();}" + labelPosition [80.125 91.875 102.544 14.964] + fontSize 12 + src { + intersection [0 0 1 0 23.5747 14.625 0 0] + } + dst { + id 23 + intersection [1 0 -1 0 23.5747 42.5747 0 0] + } + midPoint [23.5747 24.9468] + chart 18 + dataLimits [21.175 25.975 14.625 42.575] + subviewer 18 + drawStyle SMART + slide { + sticky BOTH_STICK + } + executionOrder 1 + ssIdNumber 2 + linkNode [18 0 0] + } + instance { + id 25 + machine 1 + name "Utilities/TruncPinv/Truncated PseudoInverse" + chart 18 + } + target { + id 26 + machine 1 + name "sfun" + description "Default Simulink S-Function Target." + linkNode [1 0 0] + } +} diff --git a/toolbox/matlab/library/exported/WBToolboxLibrary.slx b/toolbox/matlab/library/exported/WBToolboxLibrary.slx new file mode 100644 index 000000000..da72a0ac4 Binary files /dev/null and b/toolbox/matlab/library/exported/WBToolboxLibrary.slx differ diff --git a/toolbox/.setupForMatlabDebug.m.in b/toolbox/matlab/setupForMatlabDebug.m.in similarity index 100% rename from toolbox/.setupForMatlabDebug.m.in rename to toolbox/matlab/setupForMatlabDebug.m.in diff --git a/toolbox/matlab/slblocks.m b/toolbox/matlab/slblocks.m new file mode 100644 index 000000000..7c348193b --- /dev/null +++ b/toolbox/matlab/slblocks.m @@ -0,0 +1,8 @@ +function blkStruct = slblocks + +% Name of the .mdl file +Browser.Library = 'WBToolboxLibrary'; +Browser.Name = 'Whole Body Toolbox'; +Browser.IsFlat = 0; + +blkStruct.Browser = Browser; diff --git a/toolbox/.startup_wbitoolbox.m.in b/toolbox/matlab/startup_wbitoolbox.m.in similarity index 100% rename from toolbox/.startup_wbitoolbox.m.in rename to toolbox/matlab/startup_wbitoolbox.m.in diff --git a/toolbox/slblocks.m b/toolbox/slblocks.m deleted file mode 100644 index 89a28e382..000000000 --- a/toolbox/slblocks.m +++ /dev/null @@ -1,54 +0,0 @@ -function blkStruct = slblocks - -% Copyright (C) 2013 Robotics, Brain and Cognitive Sciences - Istituto Italiano di Tecnologia -% Author: Jorhabib Eljaik Gomez -% email: jorhabib.eljaik@iit.it -% The development of this software was supported by the FP7 EU project -% CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b)) -% http://www.codyco.eu -% Permission is granted to copy, distribute, and/or modify this program -% under the terms of the GNU General Public License, version 2 or any -% later version published by the Free Software Foundation. -% -% This program is distributed in the hope that it will be useful, but -% WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General -% Public License for more details -% Copyright (C) 2013 Robotics, Brain and Cognitive Sciences - Istituto Italiano di Tecnologia -% Author: Jorhabib Eljaik Gomez -% email: jorhabib.eljaik@iit.it -% -% The development of this software was supported by the FP7 EU project -% CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b)) -% http://www.codyco.eu -% -% Permission is granted to copy, distribute, and/or modify this program -% under the terms of the GNU General Public License, version 2 or any -% later version published by the Free Software Foundation. -% This program is distributed in the hope that it will be useful, but -% WITHOUT ANY WARRANTY; without even the implied warranty of -% MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General -% Public License for more details - -Browser.Library = 'WBToolboxLibrary'; %Name of the .mdl file -Browser.Name = 'Whole Body Toolbox'; -Browser.IsFlat = 0; - -if (~verLessThan('matlab', '8.4')) % R2014b - % Add repository information if not yet done - % ???: is this part really needed? - try - load_system(Browser.Library); - if (strcmp(get_param(Browser.Library, 'EnableLBRepository'), 'off')) - set_param(Browser.Library, 'Lock', 'off'); - set_param(Browser.Library, 'EnableLBRepository', 'on'); - set_param(Browser.Library, 'Lock', 'on'); - save_system(Browser.Library); - end; - close_system(Browser.Library); - catch ex; - end -end; - - -blkStruct.Browser = Browser; diff --git a/toolbox/src/CentroidalMomentum.cpp b/toolbox/src/CentroidalMomentum.cpp index 35a707f68..126105c9c 100644 --- a/toolbox/src/CentroidalMomentum.cpp +++ b/toolbox/src/CentroidalMomentum.cpp @@ -2,158 +2,136 @@ #include "BlockInformation.h" #include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include -#include - -namespace wbt { - - std::string CentroidalMomentum::ClassName = "CentroidalMomentum"; - - CentroidalMomentum::CentroidalMomentum() - : m_basePose(0) - , m_centroidalMomentum(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_baseVelocity(0) - , m_jointsVelocity(0) {} - - bool CentroidalMomentum::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations - - if (!blockInfo->setNumberOfInputPorts(4)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - bool success = true; - - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); //base pose - success = success && blockInfo->setInputPortVectorSize(1, dofs); //joint configuration - success = success && blockInfo->setInputPortVectorSize(2, 6); //base velocity - success = success && blockInfo->setInputPortVectorSize(3, dofs); //joints velocitity - - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); - blockInfo->setInputPortType(2, PortDataTypeDouble); - blockInfo->setInputPortType(3, PortDataTypeDouble); - - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } - - // Output port: - // - 6 vector representing the centroidal momentum - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } - - success = blockInfo->setOutputPortVectorSize(0, 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - - return success; +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include +#include + +using namespace wbt; + +const std::string CentroidalMomentum::ClassName = "CentroidalMomentum"; + +const unsigned CentroidalMomentum::INPUT_IDX_BASE_POSE = 0; +const unsigned CentroidalMomentum::INPUT_IDX_JOINTCONF = 1; +const unsigned CentroidalMomentum::INPUT_IDX_BASE_VEL = 2; +const unsigned CentroidalMomentum::INPUT_IDX_JOINT_VEL = 3; +const unsigned CentroidalMomentum::OUTPUT_IDX_CENTRMOM = 0; + +CentroidalMomentum::CentroidalMomentum() {} + +bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(4)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool CentroidalMomentum::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); + + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); + + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_centroidalMomentum = new double[6]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; - m_baseVelocity = new double[6]; - m_jointsVelocity = new double[dofs]; + // OUTPUTS + // ======= + // + // 1) Vector representing the centroidal momentum (1x6) + // - return m_basePose && m_centroidalMomentum && m_basePoseRaw && m_configuration && m_baseVelocity && m_jointsVelocity; + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } - bool CentroidalMomentum::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_centroidalMomentum) { - delete [] m_centroidalMomentum; - m_centroidalMomentum = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - if (m_baseVelocity) { - delete [] m_baseVelocity; - m_baseVelocity = 0; - } - if (m_jointsVelocity) { - delete [] m_jointsVelocity; - m_jointsVelocity = 0; - } - - return WBIModelBlock::terminate(blockInfo, error); + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_CENTRMOM, 6); + blockInfo->setOutputPortType(OUTPUT_IDX_CENTRMOM, PortDataTypeDouble); + + return success; +} + +bool CentroidalMomentum::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + // OUTPUT + // ====== + + m_centroidalMomentum = std::unique_ptr(new iDynTree::SpatialMomentum()); + return static_cast(m_centroidalMomentum); +} + +bool CentroidalMomentum::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} + +bool CentroidalMomentum::output(const BlockInformation* blockInfo) +{ + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - bool CentroidalMomentum::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - Signal baseVelocity = blockInfo->getInputPortSignal(2); - Signal jointsVelocity = blockInfo->getInputPortSignal(3); - - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(2); ++i) { - m_baseVelocity[i] = baseVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(3); ++i) { - m_jointsVelocity[i] = jointsVelocity.get(i).doubleData(); - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - interface->computeCentroidalMomentum(m_configuration, - frame, - m_jointsVelocity, - m_baseVelocity, - m_centroidalMomentum); - - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(m_centroidalMomentum, blockInfo->getOutputPortWidth(0)); - - return true; - } + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== + + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + &baseVelocitySignal, + &jointsVelocitySignal); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); return false; } + + // OUTPUT + // ====== + + // Calculate the centroidal momentum + *m_centroidalMomentum = model->getCentroidalTotalMomentum(); + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_CENTRMOM); + output.setBuffer(toEigen(*m_centroidalMomentum).data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_CENTRMOM)); + return true; } diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp new file mode 100644 index 000000000..674501a50 --- /dev/null +++ b/toolbox/src/DiscreteFilter.cpp @@ -0,0 +1,287 @@ +#include "DiscreteFilter.h" +#include "BlockInformation.h" +#include "Log.h" +#include "Signal.h" +#include +#include +#include +#include +#include +#include + +using namespace wbt; +using namespace iCub::ctrl; +using namespace yarp::sig; + +const std::string DiscreteFilter::ClassName = "DiscreteFilter"; + +// Parameters +const unsigned DiscreteFilter::PARAM_IDX_FLT_TYPE = 1; // ::Filter type +const unsigned DiscreteFilter::PARAM_IDX_NUMCOEFF = 2; // ::Filter numerator coefficients +const unsigned DiscreteFilter::PARAM_IDX_DENCOEFF = 3; // ::Filter denominator coefficients +const unsigned DiscreteFilter::PARAM_IDX_1LOWP_FC = 4; // ::FirstOrderLowPassFilter cut frequency +const unsigned DiscreteFilter::PARAM_IDX_1LOWP_TS = 5; // ::FirstOrderLowPassFilter sampling time +const unsigned DiscreteFilter::PARAM_IDX_MD_ORDER = 6; // ::MedianFilter order +const unsigned DiscreteFilter::PARAM_IDX_INIT_Y0 = 7; // Output initialization +const unsigned DiscreteFilter::PARAM_IDX_INIT_U0 = 8; // ::Filter input initialization + +// Inputs +const unsigned DiscreteFilter::INPUT_IDX_SIGNAL = 0; +// Outputs +const unsigned DiscreteFilter::OUTPUT_IDX_SIGNAL = 0; +// Other defines +const int DiscreteFilter::SIGNAL_DYNAMIC_SIZE = -1; + +DiscreteFilter::DiscreteFilter() +{} + +unsigned DiscreteFilter::numberOfParameters() +{ + return 8; +} + +bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + // Specify I/O + // =========== + + // INPUTS + // ------ + + // Number of input ports + int numberOfInputPorts = 1; + if (!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { + Log::getSingleton().error("Failed to set input port number."); + return false; + } + + // Input port sizes + blockInfo->setInputPortVectorSize(INPUT_IDX_SIGNAL, SIGNAL_DYNAMIC_SIZE); + blockInfo->setInputPortType(INPUT_IDX_SIGNAL, PortDataTypeDouble); + + // OUTPUTS + // ------- + + // Number of output ports + int numberOfOutputPorts = 1; + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } + + // Output port sizes + blockInfo->setOutputPortVectorSize(OUTPUT_IDX_SIGNAL, SIGNAL_DYNAMIC_SIZE); + blockInfo->setOutputPortType(OUTPUT_IDX_SIGNAL, PortDataTypeDouble); + + return true; +} + +bool DiscreteFilter::initialize(const BlockInformation* blockInfo) +{ + // Handle the parameters + // ===================== + + // Variables for the filter parameters + std::string filter_type; + std::string num_coeff_str; + std::string den_coeff_str; + std::string y0_str; + std::string u0_str; + double firstOrderLowPassFilter_fc; + double firstOrderLowPassFilter_ts; + double medianFilter_order; + + // Get the scalar parameters + bool ok = true; + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_FC, + firstOrderLowPassFilter_fc); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_TS, + firstOrderLowPassFilter_ts); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_MD_ORDER, + medianFilter_order); + + // Get the string parameter + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_Y0, y0_str); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_U0, u0_str); + + + if (!ok) { + Log::getSingleton().error("Failed to parse parameters."); + return false; + } + + // Convert the strings from the Matlab syntax ("[1.0 2 3.33]") to yarp::sig::Vector + yarp::sig::Vector num(0); + yarp::sig::Vector den(0); + stringToYarpVector(num_coeff_str, num); + stringToYarpVector(den_coeff_str, den); + + // y0 and u0 are none if they are not defined + unsigned y0Width, u0Width; + + if (y0_str != "none") { + y0 = std::unique_ptr(new Vector(0)); + stringToYarpVector(y0_str, *y0); + y0Width = y0->length(); + } + + if (u0_str != "none") { + u0 = std::unique_ptr(new Vector(0)); + stringToYarpVector(u0_str, *u0); + u0Width = u0->length(); + } + + // Create the filter object + // ======================== + + // Generic + // ------- + if (filter_type == "Generic") { + if (num.length() == 0 || den.length() == 0) { + Log::getSingleton().error("(Generic) Wrong coefficients size."); + return false; + } + filter = std::unique_ptr(new Filter(num, den)); + } + // FirstOrderLowPassFilter + // ----------------------- + else if (filter_type == "FirstOrderLowPassFilter") { + if (firstOrderLowPassFilter_fc == 0 || firstOrderLowPassFilter_ts == 0) { + Log::getSingleton().error("(FirstOrderLowPassFilter) You need to " + "specify Fc and Ts."); + return false; + } + filter = std::unique_ptr( + new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc, + firstOrderLowPassFilter_ts)); + } + // MedianFilter + // ------------ + else if (filter_type == "MedianFilter") { + if (static_cast(medianFilter_order) == 0) { + Log::getSingleton().error("(MedianFilter) You need to specify the " + "filter order."); + return false; + } + filter = std::unique_ptr(new MedianFilter(static_cast(medianFilter_order))); + } + else { + Log::getSingleton().error("Filter type not recognized."); + return false; + } + + // Initialize the other data + // ========================= + + // Get the width of the input vector + inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); + + // Check the initial conditions are properly sized + unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); + + if ((y0 != nullptr) && (y0Width != outputSignalWidth)) { + Log::getSingleton().error("y0 and output signal sizes don't match."); + return false; + } + + if ((u0 != nullptr) && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { + Log::getSingleton().error("(Generic) u0 and input signal sizes don't match."); + return false; + } + + // Allocate the input signal + inputSignalVector = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); + + return true; +} + +bool DiscreteFilter::terminate(const BlockInformation* blockInfo) +{ + return true; +} + +bool DiscreteFilter::initializeInitialConditions(const BlockInformation* blockInfo) +{ + // Reminder: this function is called when, during runtime, a block is disabled + // and enabled again. The method ::initialize instead is called just one time. + + // If the initial conditions for the output are not set, allocate a properly + // sized vector + if (y0 == nullptr) { + unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); + y0 = std::unique_ptr(new Vector(outputSignalWidth, 0.0)); + } + if (u0 == nullptr) { + u0 = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); + } + + // Initialize the filter. This is required because if the signal is not 1D, + // the default filter constructor initialize a wrongly sized y0 + // Moreover, the Filter class has a different constructor that handles the + // zero-gain case + Filter* filter_c = dynamic_cast(filter.get()); + if (filter_c != nullptr) { + filter_c->init(*y0, *u0); + } + else { + filter->init(*y0); + } + + return true; +} + +bool DiscreteFilter::output(const BlockInformation* blockInfo) +{ + if (filter == nullptr) return false; + + // Get the input and output signals + Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL); + Signal outputSignal = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL); + + // Copy the Signal to the data structure that the filter wants + for (unsigned i = 0; i < inputSignalWidth; ++i) { + (*inputSignalVector)[i] = inputSignal.get(i); + } + + // Filter the current component of the input signal + const Vector& outputVector = filter->filt(*inputSignalVector); + + // Forward the filtered signals to the output port + outputSignal.setBuffer(outputVector.data(), outputVector.length()); + + return true; +} + +void DiscreteFilter::stringToYarpVector(const std::string str, Vector& v) +{ + std::string s = str; + + // Lambda expression used to remove "[]," carachters + // TODO: what about removing everything but digits and "."? + auto lambda_remove_chars = [](const char& c) { + if ((c == '[') || (c == ']') || c == ',') + return true; + else + return false; + }; + + // Apply the lambda expression the input parameters + s.erase(remove_if(s.begin(), s.end(), lambda_remove_chars), s.end()); + + // Convert the cleaned string to a yarp vector of floats + std::istringstream sstrm(s); + + // Avoid problems that may arise due to system's locale + // https://github.com/robotology/idyntree/issues/288 + sstrm.imbue(std::locale::classic()); + + float f; + + while (sstrm >> f) + v.push_back(f); +} diff --git a/toolbox/src/DotJNu.cpp b/toolbox/src/DotJNu.cpp index 1dfaef8e8..8beab3e06 100644 --- a/toolbox/src/DotJNu.cpp +++ b/toolbox/src/DotJNu.cpp @@ -1,186 +1,183 @@ #include "DotJNu.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include -#include -#include - -namespace wbt { - - std::string DotJNu::ClassName = "DotJNu"; - - DotJNu::DotJNu() - : m_basePose(0) - , m_dotJNu(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_baseVelocity(0) - , m_jointsVelocity(0) - , m_frameIndex(-1) {} - - unsigned DotJNu::numberOfParameters() - { - return WBIBlock::numberOfParameters() + 1; +#include "RobotInterface.h" +#include +#include +#include + +using namespace wbt; + +const std::string DotJNu::ClassName = "DotJNu"; + +const unsigned DotJNu::INPUT_IDX_BASE_POSE = 0; +const unsigned DotJNu::INPUT_IDX_JOINTCONF = 1; +const unsigned DotJNu::INPUT_IDX_BASE_VEL = 2; +const unsigned DotJNu::INPUT_IDX_JOINT_VEL = 3; +const unsigned DotJNu::OUTPUT_IDX_DOTJ_NU = 0; + +DotJNu::DotJNu() +: m_frameIsCoM(false) +, m_frameIndex(iDynTree::FRAME_INVALID_INDEX) +{} + +unsigned DotJNu::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} + +bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(4)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool DotJNu::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); - if (!blockInfo->setNumberOfInputPorts(4)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - bool success = true; + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); //base pose - success = success && blockInfo->setInputPortVectorSize(1, dofs); //joint configuration - success = success && blockInfo->setInputPortVectorSize(2, 6); //base velocity - success = success && blockInfo->setInputPortVectorSize(3, dofs); //joints velocitity + // OUTPUTS + // ======= + // + // 1) Vector representing the \dot{J} \nu vector (1x6) + // - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); - blockInfo->setInputPortType(2, PortDataTypeDouble); - blockInfo->setInputPortType(3, PortDataTypeDouble); + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_DOTJ_NU, 6); + blockInfo->setOutputPortType(OUTPUT_IDX_DOTJ_NU, PortDataTypeDouble); - // Output port: - // - 6-d vector representing the \dot{J} \dot{q} vector - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } + return success; +} + +bool DotJNu::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - success = blockInfo->setOutputPortVectorSize(0, 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // INPUT PARAMETERS + // ================ - return success; + std::string frame; + int parentParameters = WBBlock::numberOfParameters(); + + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); + return false; } - bool DotJNu::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + // Check if the frame is valid + // --------------------------- - int parentParameters = WBIBlock::numberOfParameters() + 1; - std::string frame; - if (!blockInfo->getStringParameterAtIndex(parentParameters, frame)) { - if (error) error->message = "Cannot retrieve string from frame parameter"; - return false; - } - //here obtain joint list and get the frame - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (!interface) { - if (error) error->message = "Cannot retrieve handle to WBI interface"; + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); + return false; + } + + if (frame != "com") { + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; } - wbi::IDList frames = interface->getFrameList(); - if (frame != "com") { - if (!frames.idToIndex(wbi::ID(frame), m_frameIndex)) { - if (error) error->message = "Cannot find " + frame + " in the frame list"; - return false; - } - } else { - m_frameIndex = wbi::wholeBodyInterface::COM_LINK_ID; - } - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_dotJNu = new double[6]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; - m_baseVelocity = new double[6]; - m_jointsVelocity = new double[dofs]; - - return m_basePose && m_dotJNu && m_basePoseRaw && m_configuration && m_baseVelocity && m_jointsVelocity; + } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; } - bool DotJNu::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_dotJNu) { - delete [] m_dotJNu; - m_dotJNu = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - if (m_baseVelocity) { - delete [] m_baseVelocity; - m_baseVelocity = 0; - } - if (m_jointsVelocity) { - delete [] m_jointsVelocity; - m_jointsVelocity = 0; - } - return WBIModelBlock::terminate(blockInfo, error); + // OUTPUT + // ====== + m_dotJNu = std::unique_ptr(new iDynTree::Vector6()); + m_dotJNu->zero(); + + return static_cast(m_dotJNu); +} + +bool DotJNu::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} + +bool DotJNu::output(const BlockInformation* blockInfo) +{ + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - bool DotJNu::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - Signal baseVelocity = blockInfo->getInputPortSignal(2); - Signal jointsVelocity = blockInfo->getInputPortSignal(3); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(2); ++i) { - m_baseVelocity[i] = baseVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(3); ++i) { - m_jointsVelocity[i] = jointsVelocity.get(i).doubleData(); - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - interface->computeDJdq(m_configuration, - frame, - m_jointsVelocity, - m_baseVelocity, - m_frameIndex, - m_dotJNu); - - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(m_dotJNu, blockInfo->getOutputPortWidth(0)); - - return true; - } + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== + + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + &baseVelocitySignal, + &jointsVelocitySignal); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); return false; } + + // OUTPUT + // ====== + + if (!m_frameIsCoM) { + *m_dotJNu = model->getFrameBiasAcc(m_frameIndex); + } + else { + iDynTree::Vector3 comBiasAcc = model->getCenterOfMassBiasAcc(); + toEigen(*m_dotJNu).segment<3>(0) = iDynTree::toEigen(comBiasAcc); + toEigen(*m_dotJNu).segment<3>(3).setZero(); + } + + // Forward the output to Simulink + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_DOTJ_NU); + output.setBuffer(m_dotJNu->data(), + blockInfo->getOutputPortWidth(OUTPUT_IDX_DOTJ_NU)); + return true; } diff --git a/toolbox/src/ForwardKinematics.cpp b/toolbox/src/ForwardKinematics.cpp index 81f7dee14..aca145cac 100644 --- a/toolbox/src/ForwardKinematics.cpp +++ b/toolbox/src/ForwardKinematics.cpp @@ -2,160 +2,179 @@ #include "BlockInformation.h" #include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include #include -namespace wbt { +using namespace wbt; - std::string ForwardKinematics::ClassName = "ForwardKinematics"; +const std::string ForwardKinematics::ClassName = "ForwardKinematics"; - ForwardKinematics::ForwardKinematics() - : m_basePose(0) - , m_frameForwardKinematics(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_frameIndex(-1) {} +const unsigned ForwardKinematics::INPUT_IDX_BASE_POSE = 0; +const unsigned ForwardKinematics::INPUT_IDX_JOINTCONF = 1; +const unsigned ForwardKinematics::OUTPUT_IDX_FW_FRAME = 0; - unsigned ForwardKinematics::numberOfParameters() - { - return WBIBlock::numberOfParameters() + 1; +ForwardKinematics::ForwardKinematics() +: m_frameIsCoM(false) +, m_frameIndex(iDynTree::FRAME_INVALID_INDEX) +{} + +unsigned ForwardKinematics::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} + +bool ForwardKinematics::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool ForwardKinematics::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - if (!blockInfo->setNumberOfInputPorts(2)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); - success = success && blockInfo->setInputPortVectorSize(1, dofs); + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); + // OUTPUTS + // ======= + // + // 1) Homogeneous transformation between the world and the specified frame (4x4 matrix) + // - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - // Output port: - // - (4)x(4) matrix representing the homogenous transformation between the specified frame and the world frame - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 4, 4); + blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); + + return success; +} - success = blockInfo->setOutputPortMatrixSize(0, 4, 4); - blockInfo->setOutputPortType(0, PortDataTypeDouble); +bool ForwardKinematics::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - return success; + // INPUT PARAMETERS + // ================ + + std::string frame; + int parentParameters = WBBlock::numberOfParameters(); + + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); + return false; } - bool ForwardKinematics::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + // Check if the frame is valid + // --------------------------- - int parentParameters = WBIBlock::numberOfParameters() + 1; - std::string frame; - if (!blockInfo->getStringParameterAtIndex(parentParameters, frame)) { - if (error) error->message = "Cannot retrieve string from frame parameter"; - return false; - } + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to WBI model."); + return false; + } - //here obtain joint list and get the frame - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (!interface) { - if (error) error->message = "Cannot retrieve handle to WBI interface"; + if (frame != "com") { + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; } - wbi::IDList frames = interface->getFrameList(); - if (frame != "com") { - if (!frames.idToIndex(wbi::ID(frame), m_frameIndex)) { - if (error) error->message = "Cannot find " + frame + " in the frame list"; - return false; - } - } else { - m_frameIndex = wbi::wholeBodyInterface::COM_LINK_ID; - } + } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; + } - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_frameForwardKinematics = new double[4 * 4]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; + return true; +} - return m_basePose && m_frameForwardKinematics && m_basePoseRaw && m_configuration; - } +bool ForwardKinematics::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} - bool ForwardKinematics::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_frameForwardKinematics) { - delete [] m_frameForwardKinematics; - m_frameForwardKinematics = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - return WBIModelBlock::terminate(blockInfo, error); +bool ForwardKinematics::output(const BlockInformation* blockInfo) +{ + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + typedef Matrix Matrix4diDynTree; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - bool ForwardKinematics::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1) - ; - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - wbi::Frame outputFrame; - interface->computeH(m_configuration, frame, m_frameIndex, outputFrame); - outputFrame.get4x4Matrix(m_frameForwardKinematics); - - Eigen::Map > frameRowMajor(m_frameForwardKinematics); - - Signal output = blockInfo->getOutputPortSignal(0); - Eigen::Map > frameColMajor((double*)output.getContiguousBuffer(), 4, 4); - frameColMajor = frameRowMajor; - return true; - } + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== + + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + nullptr, + nullptr); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); return false; } + + // OUTPUT + // ====== + + iDynTree::Transform world_H_frame; + + // Compute the transform to the selected frame + if (!m_frameIsCoM) { + world_H_frame = model->getWorldTransform(m_frameIndex); + } + else { + world_H_frame.setPosition(model->getCenterOfMassPosition()); + } + + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); + + // Allocate objects for row-major -> col-major conversion + Map world_H_frame_RowMajor = toEigen(world_H_frame.asHomogeneousTransform()); + Map world_H_frame_ColMajor(output.getBuffer(), + 4, 4); + + // Forward the buffer to Simulink transforming it to ColMajor + world_H_frame_ColMajor = world_H_frame_RowMajor; + return true; } diff --git a/toolbox/src/GetEstimate.cpp b/toolbox/src/GetEstimate.cpp deleted file mode 100644 index ec2ed17c9..000000000 --- a/toolbox/src/GetEstimate.cpp +++ /dev/null @@ -1,106 +0,0 @@ -#include "GetEstimate.h" - -#include "Error.h" -#include "WBInterface.h" -#include "BlockInformation.h" -#include "Signal.h" -#include -#include - -namespace wbt { - - std::string GetEstimate::ClassName = "GetEstimate"; - - GetEstimate::GetEstimate() - : m_estimate(0) - , m_estimateType(wbi::ESTIMATE_JOINT_POS) {} - - unsigned GetEstimate::numberOfParameters() - { - return WBIBlock::numberOfParameters() + 1; - } - - bool GetEstimate::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - // Output port: - // - DoFs vector with the information asked - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } - - bool success = blockInfo->setOutputPortVectorSize(0, dofs); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - if (!success) { - if (error) error->message = "Failed to configure output ports"; - return false; - } - - - return true; - } - - bool GetEstimate::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_estimate = new double[dofs]; - - // Reading the control type - std::string informationType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, informationType)) { - if (error) error->message = "Could not read estimate type parameter"; - return false; - } - - if (informationType == "Joints Position") { - m_estimateType = wbi::ESTIMATE_JOINT_POS; - } else if (informationType == "Joints Velocity") { - m_estimateType = wbi::ESTIMATE_JOINT_VEL; - } else if (informationType == "Joints Acceleration") { - m_estimateType = wbi::ESTIMATE_JOINT_ACC; - } else if (informationType == "Joints Torque") { - m_estimateType = wbi::ESTIMATE_JOINT_TORQUE; - } else { - if (error) error->message = "Estimate not supported"; - return false; - } - return m_estimate; - } - - bool GetEstimate::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_estimate) { - delete [] m_estimate; - m_estimate = 0; - } - return WBIBlock::terminate(blockInfo, error); - } - - bool GetEstimate::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - //get input - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); - if (interface) { - interface->getEstimates(m_estimateType, m_estimate); - Signal signal = blockInfo->getOutputPortSignal(0); - signal.setBuffer(m_estimate, blockInfo->getOutputPortWidth(0)); - - return true; - } - return false; - } -} diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index 8ad4a6093..ca668fad3 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -1,131 +1,234 @@ #include "GetLimits.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include "WBInterface.h" -#include -#include +#include "RobotInterface.h" +#include +#include +#include +#include +#include -namespace wbt { +#define _USE_MATH_DEFINES +#include - std::string GetLimits::ClassName = "GetLimits"; +using namespace wbt; - struct GetLimits::Limit { - double *min; - double *max; +const std::string GetLimits::ClassName = "GetLimits"; - Limit(unsigned size) : min(0), max(0) - { - min = new double[size](); - max = new double[size](); - } +double GetLimits::deg2rad(const double& v) +{ + return v * M_PI / 180.0; +} - ~Limit() - { - if (min) { - delete [] min; - min = 0; - } - if (max) { - delete [] max; - max = 0; - } - } - }; +unsigned GetLimits::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} - GetLimits::GetLimits() - : m_limits(0) {} +bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - unsigned GetLimits::numberOfParameters() - { - return WBIBlock::numberOfParameters() + 1; + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - bool GetLimits::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // OUTPUTS + // ======= + // + // 1) vector with the information asked (1xDoFs) + // + + if (!blockInfo->setNumberOfOutputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); + bool success = true; + success = success && blockInfo->setOutputPortVectorSize(0, dofs); // Min limit + success = success && blockInfo->setOutputPortVectorSize(1, dofs); // Max limit - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to configure the number of input ports"; + blockInfo->setOutputPortType(0, PortDataTypeDouble); + blockInfo->setOutputPortType(1, PortDataTypeDouble); + + if (!success) { + Log::getSingleton().error("Failed to configure output ports."); + return false; + } + + return true; +} + +bool GetLimits::initialize(const BlockInformation* blockInfo) +{ + using namespace yarp::os; + if (!WBBlock::initialize(blockInfo)) return false; + + // Read the control type + std::string limitType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType)) { + Log::getSingleton().error("Could not read estimate type parameter."); + return false; + } + + // Initialize the structure that stores the limits + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_limits.reset(new Limit(dofs)); + + // Initializes some buffers + double min = 0; + double max = 0; + + // From the RemoteControlBoardRemapper + // =================================== + // + // In the next methods, the values are asked using joint index and not string. + // The ControlBoardRemapper internally uses the same joints ordering of its + // initialization. In this case, it matches 1:1 the joint vector. It is hence + // possible using i to point to the correct joint. + + // Get the RemoteControlBoardRemapper and IControlLimits2 interface if needed + yarp::dev::IControlLimits2* iControlLimits2 = nullptr; + if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { + // Retain the control board remapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); return false; } - - // Output port: - // - DoFs vector with the information asked - if (!blockInfo->setNumberOfOuputPorts(2)) { - if (error) error->message = "Failed to configure the number of output ports"; + // Get the interface + if (!getRobotInterface()->getInterface(iControlLimits2) || !iControlLimits2) { + Log::getSingleton().error("Failed to get IControlLimits2 interface."); return false; } + } - bool success = blockInfo->setOutputPortVectorSize(0, dofs); //Min limit - success = success && blockInfo->setOutputPortVectorSize(1, dofs); //Max limit + if (limitType == "ControlBoardPosition") { + for (auto i = 0; i < dofs; ++i) { + if (!iControlLimits2->getLimits(i, &min, &max)) { + Log::getSingleton().error("Failed to get limits from the interface."); + return false; + } + m_limits->min[i] = deg2rad(min); + m_limits->max[i] = deg2rad(max); + } + } + else if (limitType == "ControlBoardVelocity") { + for (auto i = 0; i < dofs; ++i) { + if (!iControlLimits2->getVelLimits(i, &min, &max)) { + Log::getSingleton().error("Failed to get limits from the interface."); + return false; + } + m_limits->min[i] = deg2rad(min); + m_limits->max[i] = deg2rad(max); + } + } + + // From the URDF model + // =================== + // + // For the time being, only position limits are supported. - blockInfo->setOutputPortType(0, PortDataTypeDouble); - blockInfo->setOutputPortType(1, PortDataTypeDouble); + else if (limitType == "ModelPosition") { + iDynTree::IJointConstPtr p_joint; - if (!success) { - if (error) error->message = "Failed to configure output ports"; + // Get the KinDynComputations pointer + const auto& kindyncomp = getRobotInterface()->getKinDynComputations(); + if (!kindyncomp) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); return false; } + // Get the model + const iDynTree::Model model = kindyncomp->model(); - return true; - } + for (auto i = 0; i < dofs; ++i) { + // Get the joint name + std::string joint = getConfiguration().getControlledJoints()[i]; - bool GetLimits::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; + // Get its index + iDynTree::JointIndex jointIndex = model.getJointIndex(joint); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); + if (jointIndex == iDynTree::JOINT_INVALID_INDEX) { + Log::getSingleton().error("Invalid iDynTree joint index."); + return false; + } - // Reading the control type - std::string limitType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, limitType)) { - if (error) error->message = "Could not read estimate type parameter"; - return false; - } + // Get the joint from the model + p_joint = model.getJoint(jointIndex); - bool success = true; - if (limitType == "Position") { - m_limits = new Limit(dofs); - if (m_limits) { - success = interface->getJointLimits(m_limits->min, m_limits->max); + if (!p_joint->hasPosLimits()) { + Log::getSingleton().warning("Joint " + joint + " has no model limits."); + m_limits->min[i] = -std::numeric_limits::infinity(); + m_limits->max[i] = std::numeric_limits::infinity(); + } + else { + if (!p_joint->getPosLimits(0, min, max)) { + Log::getSingleton().error("Failed to get joint limits from the URDF model "); + Log::getSingleton().errorAppend("for the joint " + joint + "."); + return false; + } + m_limits->min[i] = min; + m_limits->max[i] = max; } - } else { - if (error) error->message = "Limit type not supported"; - return false; } + } + // TODO + // else if (limitType == "ModelVelocity") { + // } + // else if (limitType == "ModelEffort") { + // } + else { + Log::getSingleton().error("Limit type " + limitType + " not recognized."); + return false; + } + + return true; +} + +bool GetLimits::terminate(const BlockInformation* blockInfo) +{ + bool ok = true; - return m_limits && success; + // Read the control type + std::string limitType; + ok = ok && blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType); + if (!ok) { + Log::getSingleton().error("Could not read estimate type parameter."); + // Don't return false here. WBBlock::terminate must be called in any case } - bool GetLimits::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_limits) { - delete m_limits; - m_limits = 0; + // Release the RemoteControlBoardRemapper + if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case } - return WBIBlock::terminate(blockInfo, error); } - bool GetLimits::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - if (!m_limits) return false; + return ok && WBBlock::terminate(blockInfo); +} - Signal minPort = blockInfo->getOutputPortSignal(0); - Signal maxPort = blockInfo->getOutputPortSignal(1); +bool GetLimits::output(const BlockInformation* blockInfo) +{ + if (!m_limits) return false; - for (int i = 0; i < blockInfo->getOutputPortWidth(0); ++i) { - minPort.set(i, m_limits->min[i]); - maxPort.set(i, m_limits->max[i]); - } - return true; - } + Signal minPort = blockInfo->getOutputPortSignal(0); + Signal maxPort = blockInfo->getOutputPortSignal(1); + + minPort.setBuffer(m_limits->min.data(), getConfiguration().getNumberOfDoFs()); + maxPort.setBuffer(m_limits->max.data(), getConfiguration().getNumberOfDoFs()); + + return true; } diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp new file mode 100644 index 000000000..e95e31dad --- /dev/null +++ b/toolbox/src/GetMeasurement.cpp @@ -0,0 +1,183 @@ +#include "GetMeasurement.h" + +#include "Log.h" +#include "BlockInformation.h" +#include "Signal.h" +#include "RobotInterface.h" +#include +#include + +#define _USE_MATH_DEFINES +#include + +using namespace wbt; + +const std::string GetMeasurement::ClassName = "GetMeasurement"; + +void GetMeasurement::deg2rad(std::vector& v) +{ + const double Deg2Rad = M_PI / 180.0; + for (auto& element : v) { + element *= Deg2Rad; + } +} + +unsigned GetMeasurement::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} + +bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } + + // OUTPUTS + // ======= + // + // 1) Vector with the information asked (1xDoFs) + // + + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + bool success = blockInfo->setOutputPortVectorSize(0, dofs); + blockInfo->setOutputPortType(0, PortDataTypeDouble); + if (!success) { + Log::getSingleton().error("Failed to configure output ports."); + return false; + } + + return true; +} + +bool GetMeasurement::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + // Reading the control type + std::string informationType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, informationType)) { + Log::getSingleton().error("Could not read estimate type parameter."); + return false; + } + + if (informationType == "Joints Position") { + m_measuredType = wbt::MEASUREMENT_JOINT_POS; + } else if (informationType == "Joints Velocity") { + m_measuredType = wbt::MEASUREMENT_JOINT_VEL; + } else if (informationType == "Joints Acceleration") { + m_measuredType = wbt::MEASUREMENT_JOINT_ACC; + } else if (informationType == "Joints Torque") { + m_measuredType = wbt::ESTIMATE_JOINT_TORQUE; + } else { + Log::getSingleton().error("Estimate not supported."); + return false; + } + + // Initialize the size of the output vector + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_measurement.resize(dofs); + + // Retain the ControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); + return false; + } + + return true; +} + +bool GetMeasurement::terminate(const BlockInformation* blockInfo) +{ + // Release the RemoteControlBoardRemapper + bool ok = true; + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case + } + + return ok && WBBlock::terminate(blockInfo); +} + +bool GetMeasurement::output(const BlockInformation* blockInfo) +{ + bool ok; + switch (m_measuredType) { + case MEASUREMENT_JOINT_POS: { + // Get the interface + yarp::dev::IEncoders* iEncoders = nullptr; + if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { + Log::getSingleton().error("Failed to get IPidControl interface."); + return false; + } + // Get the measurement + ok = iEncoders->getEncoders(m_measurement.data()); + deg2rad(m_measurement); + break; + } + case MEASUREMENT_JOINT_VEL: { + // Get the interface + yarp::dev::IEncoders* iEncoders = nullptr; + if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { + Log::getSingleton().error("Failed to get IEncoders interface."); + return false; + } + // Get the measurement + ok = iEncoders->getEncoderSpeeds(m_measurement.data()); + deg2rad(m_measurement); + break; + } + case MEASUREMENT_JOINT_ACC: { + // Get the interface + yarp::dev::IEncoders* iEncoders = nullptr; + if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { + Log::getSingleton().error("Failed to get IEncoders interface."); + return false; + } + // Get the measurement + ok = iEncoders->getEncoderAccelerations(m_measurement.data()); + deg2rad(m_measurement); + break; + } + case ESTIMATE_JOINT_TORQUE: { + // Get the interface + yarp::dev::ITorqueControl* iTorqueControl = nullptr; + if (!getRobotInterface()->getInterface(iTorqueControl) || !iTorqueControl) { + Log::getSingleton().error("Failed to get ITorqueControl interface."); + return false; + } + // Get the measurement + ok = iTorqueControl->getTorques(m_measurement.data()); + break; + } + default: + Log::getSingleton().error("Estimate type not recognized."); + return false; + } + + if (!ok) { + Log::getSingleton().error("Failed to get estimate."); + return false; + } + + Signal signal = blockInfo->getOutputPortSignal(0); + signal.setBuffer(m_measurement.data(), blockInfo->getOutputPortWidth(0)); + + return true; +} diff --git a/toolbox/src/InverseDynamics.cpp b/toolbox/src/InverseDynamics.cpp index 45999d7cc..d5d0ab1fa 100644 --- a/toolbox/src/InverseDynamics.cpp +++ b/toolbox/src/InverseDynamics.cpp @@ -1,221 +1,209 @@ #include "InverseDynamics.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include "WBInterface.h" -#include -#include +#include "RobotInterface.h" +#include +#include +#include +#include #include -#define PARAM_IDX_1 5 - -namespace wbt { - - std::string InverseDynamics::ClassName = "InverseDynamics"; - - InverseDynamics::InverseDynamics() - : m_basePose(0) - , m_torques(0) - , m_explicitGravity(false) - , m_basePoseRaw(0) - , m_configuration(0) - , m_baseVelocity(0) - , m_jointsVelocity(0) - , m_baseAcceleration(0) - , m_jointsAcceleration(0) - { - m_gravity[0] = 0; - m_gravity[1] = 0; - m_gravity[2] = -9.81; +using namespace wbt; + +const std::string InverseDynamics::ClassName = "InverseDynamics"; + +const unsigned InverseDynamics::INPUT_IDX_BASE_POSE = 0; +const unsigned InverseDynamics::INPUT_IDX_JOINTCONF = 1; +const unsigned InverseDynamics::INPUT_IDX_BASE_VEL = 2; +const unsigned InverseDynamics::INPUT_IDX_JOINT_VEL = 3; +const unsigned InverseDynamics::INPUT_IDX_BASE_ACC = 4; +const unsigned InverseDynamics::INPUT_IDX_JOINT_ACC = 5; +const unsigned InverseDynamics::OUTPUT_IDX_TORQUES = 0; + +InverseDynamics::InverseDynamics() {} + +unsigned InverseDynamics::numberOfParameters() +{ + return WBBlock::numberOfParameters(); +} + +bool InverseDynamics::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here + + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // 4) Joints velocity (1xDoFs vector) + // 5) Base frame acceleration (1x6 vector) + // 6) Joints acceleration (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(6)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } + + // Get the DoFs + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_VEL, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_VEL, dofs); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_BASE_ACC, 6); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINT_ACC, dofs); + + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_VEL, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_ACC, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINT_ACC, PortDataTypeDouble); + + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; } - unsigned InverseDynamics::numberOfParameters() - { - return WBIBlock::numberOfParameters() + 1; + // OUTPUTS + // ======= + // + // 1) Vector representing the torques (1x(DoFs+6)) + // + + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } - bool InverseDynamics::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations - - m_explicitGravity = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).booleanData(); - - int portNumber = 6; - if (m_explicitGravity) portNumber++; - - if (!blockInfo->setNumberOfInputPorts(portNumber)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - bool success = true; - - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); //base pose - success = success && blockInfo->setInputPortVectorSize(1, dofs); //joint configuration - success = success && blockInfo->setInputPortVectorSize(2, 6); //base velocity - success = success && blockInfo->setInputPortVectorSize(3, dofs); //joints velocitity - success = success && blockInfo->setInputPortVectorSize(4, 6); //base acceleration - success = success && blockInfo->setInputPortVectorSize(5, dofs); //joints acceleration - if (m_explicitGravity) - success = success && blockInfo->setInputPortVectorSize(6, 3); //gravity acceleration - - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); - blockInfo->setInputPortType(2, PortDataTypeDouble); - blockInfo->setInputPortType(3, PortDataTypeDouble); - blockInfo->setInputPortType(4, PortDataTypeDouble); - blockInfo->setInputPortType(5, PortDataTypeDouble); - - if (m_explicitGravity) - blockInfo->setInputPortType(6, PortDataTypeDouble); - - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } - - // Output port: - // - DoFs + 6) vector representing the torques - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } - - success = blockInfo->setOutputPortVectorSize(0, dofs + 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - - return success; + // Size and type + success = blockInfo->setOutputPortVectorSize(OUTPUT_IDX_TORQUES, dofs + 6); + blockInfo->setOutputPortType(OUTPUT_IDX_TORQUES, PortDataTypeDouble); + + return success; +} + +bool InverseDynamics::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + // OUTPUT / VARIABLES + // ================== + + using namespace iDynTree; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + m_baseAcceleration = std::unique_ptr(new Vector6()); + m_baseAcceleration->zero(); + m_jointsAcceleration = std::unique_ptr(new VectorDynSize(dofs)); + m_jointsAcceleration->zero(); + + // Get the KinDynComputations pointer + const auto& kindyncomp = getRobotInterface()->getKinDynComputations(); + + // Get the model from the KinDynComputations object + const auto& model = kindyncomp->model(); + + m_torques = std::unique_ptr(new FreeFloatingGeneralizedTorques(model)); + + return static_cast(m_baseAcceleration) && + static_cast(m_jointsAcceleration) && + static_cast(m_torques); +} + +bool InverseDynamics::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} + +bool InverseDynamics::output(const BlockInformation* blockInfo) +{ + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== + + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); + Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + &baseVelocitySignal, + &jointsVelocitySignal); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); + return false; } - bool InverseDynamics::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + // Base acceleration + // ----------------- - m_explicitGravity = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).booleanData(); + Signal baseAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_ACC); + double* bufBaseAcc = baseAccelerationSignal.getBuffer(); + if (!bufBaseAcc) { + Log::getSingleton().error("Failed to read data from input port."); + return false; + } + for (auto i = 0; i < baseAccelerationSignal.getWidth(); ++i) { + m_baseAcceleration->setVal(i, bufBaseAcc[i]); + } - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_torques = new double[6 + dofs]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; - m_baseVelocity = new double[6]; - m_jointsVelocity = new double[dofs]; - m_baseAcceleration = new double[6]; - m_jointsAcceleration = new double[dofs]; + // Joints acceleration + // ------------------- - return m_basePose && m_torques && m_basePoseRaw && m_configuration && m_baseVelocity && m_jointsVelocity && m_baseAcceleration && m_jointsAcceleration; + Signal jointsAccelerationSignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_ACC); + double* bufJointsAcc = jointsAccelerationSignal.getBuffer(); + if (!bufJointsAcc) { + Log::getSingleton().error("Failed to read data from input port."); + return false; + } + for (auto i = 0; i < jointsAccelerationSignal.getWidth(); ++i) { + m_jointsAcceleration->setVal(i, bufJointsAcc[i]); } - bool InverseDynamics::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_torques) { - delete [] m_torques; - m_torques = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - if (m_baseVelocity) { - delete [] m_baseVelocity; - m_baseVelocity = 0; - } - if (m_jointsVelocity) { - delete [] m_jointsVelocity; - m_jointsVelocity = 0; - } - - if (m_baseAcceleration) { - delete [] m_baseAcceleration; - m_baseAcceleration = 0; - } - - if (m_jointsAcceleration) { - delete [] m_jointsAcceleration; - m_jointsAcceleration = 0; - } - - return WBIModelBlock::terminate(blockInfo, error); + // OUTPUT + // ====== + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - bool InverseDynamics::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - Signal baseVelocity = blockInfo->getInputPortSignal(2); - Signal jointsVelocity = blockInfo->getInputPortSignal(3); - Signal baseAcceleration = blockInfo->getInputPortSignal(4); - Signal jointsAcceleration = blockInfo->getInputPortSignal(5); - - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(2); ++i) { - m_baseVelocity[i] = baseVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(3); ++i) { - m_jointsVelocity[i] = jointsVelocity.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(4); ++i) { - m_baseAcceleration[i] = baseAcceleration.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(5); ++i) { - m_jointsAcceleration[i] = jointsAcceleration.get(i).doubleData(); - } - - if (m_explicitGravity) { - Signal gravity = blockInfo->getInputPortSignal(6); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(6); ++i) { - m_gravity[i] = gravity.get(i).doubleData(); - } - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - interface->inverseDynamics(m_configuration, - frame, - m_jointsVelocity, - m_baseVelocity, - m_jointsAcceleration, - m_baseAcceleration, - m_gravity, - m_torques); - - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(m_torques, blockInfo->getOutputPortWidth(0)); - - return true; - } + // Calculate the inverse dynamics (assuming zero external forces) + model->inverseDynamics(*m_baseAcceleration, + *m_jointsAcceleration, + iDynTree::LinkNetExternalWrenches(model->getNrOfLinks()), + *m_torques); + + // Get the output signal + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_TORQUES); + double* outputBuffer = output.getBuffer(); + if (!outputBuffer) { + Log::getSingleton().error("Failed to get output buffer."); return false; } + + // Convert generalized torques and forward the directly to Simulink + // mapping the memory through Eigen::Map + const auto& torquesSize = m_torques->jointTorques().size(); + Eigen::Map generalizedOutputTrqs(outputBuffer, torquesSize + 6); + generalizedOutputTrqs.segment(0, 6) = toEigen(m_torques->baseWrench()); + generalizedOutputTrqs.segment(6, torquesSize) = toEigen(m_torques->jointTorques()); + + return true; } diff --git a/toolbox/src/InverseKinematics.cpp b/toolbox/src/InverseKinematics.cpp index 1ad048a52..f8f6e61ff 100644 --- a/toolbox/src/InverseKinematics.cpp +++ b/toolbox/src/InverseKinematics.cpp @@ -14,19 +14,13 @@ #include #include -//These includes are to convert from URDF to DH -#include -#include -#include -#include -#include -#include -#include -//End of temporary include section +#include +#include +#include -namespace wbt { +#include - static bool iKinLimbFromUrdfFile(const std::string &urdf_file_name, const std::string &base_link_name, const std::string& end_effector_link_name, iCub::iKin::iKinLimb &convertedChain, std::vector& jointNames); +namespace wbt { struct InverseKinematics::InverseKinematicsPimpl { bool m_firstTime; @@ -204,11 +198,32 @@ namespace wbt { if (!m_piml->m_leg) return false; std::vector jointNames; - if (!iKinLimbFromUrdfFile(urdfFile, baseLink, endEffectorLink, *((iCub::iKin::iKinLimb*)m_piml->m_leg), jointNames)) { - if (error) error->message = "Cannot convert urdf to iKin chain"; + iDynTree::ModelLoader loader; + loader.loadModelFromFile(urdfFile); + if (!loader.isValid()) { + if (error) error->message = "Cannot load urdf file"; + return false; + } + + iDynTree::DHChain dhChain; + if (!iDynTree::ExtractDHChainFromModel(loader.model(), + baseLink, + endEffectorLink, + dhChain)) { + if (error) error->message = "Cannot extract DH parameters from model"; + return false; + } + if (!iDynTree::iKinLimbFromDHChain(dhChain, *((iCub::iKin::iKinLimb*)m_piml->m_leg))) { + if (error) error->message = "Cannot convert DH parameters to iKin chain"; return false; } + // Retrieve joint names + jointNames.reserve(dhChain.getNrOfDOFs()); + for (size_t i = 0; i < dhChain.getNrOfDOFs(); ++i) { + jointNames.push_back(dhChain.getDOFName(i)); + } + m_piml->m_solver = new iCub::iKin::iKinIpOptMin(*m_piml->m_leg, IKINCTRL_POSE_XYZ, 1e-3, 1e-6, 100); if (!m_piml->m_solver) return false; @@ -329,114 +344,4 @@ namespace wbt { return false; } - - bool iKinLimbFromUrdfFile(const std::string &urdf_file_name, const std::string &base_link_name, const std::string& end_effector_link_name, iCub::iKin::iKinLimb &convertedChain, std::vector& jointNames) { - - KDL::Tree kdl_tree; - KDL::Chain kdl_chain; - std::vector joint_names; - KDL::JntArray min,max; - - // - // URDF --> KDL::Tree - // - bool root_inertia_workaround = true; - if (!iDynTree::treeFromUrdfFile(urdf_file_name,kdl_tree,root_inertia_workaround)) - { - std::cerr << "Could not parse urdf robot model" << std::endl; - std::cerr << "Please open an issue at https://github.com/robotology-playground/idyntree/issues " << std::endl; - - return false; - } - - // - // URDF --> position ranges - // - if (!iDynTree::jointPosLimitsFromUrdfFile(urdf_file_name,joint_names,min,max)) - { - std::cerr << "Could not parse urdf robot model limits" << std::endl; - return false; - } - - if (joint_names.size() != min.rows() || - joint_names.size() != max.rows() || - joint_names.size() == 0) - { - std::cerr << "Inconsistent joint limits got from urdf (nr of joints extracted: " << joint_names.size() << " ) " << std::endl; - return false; - } - - // - // KDL::Tree --> KDL::CoDyCo::UndirectedTree - // (for extracting arbitrary chains, - // using KDL::Tree you can just get chains where the base of the chain - // is proximal to the tree base with respect to the end effector. - // - KDL::CoDyCo::UndirectedTree undirected_tree(kdl_tree); - - KDL::Tree kdl_rotated_tree = undirected_tree.getTree(base_link_name); - - bool result = kdl_rotated_tree.getChain(base_link_name,end_effector_link_name,kdl_chain); - if (!result) - { - std::cerr << "Impossible to find " << base_link_name << " or " - << end_effector_link_name << " in the URDF." << std::endl; - return false; - } - - // - // Copy the limits extracted from the URDF to the chain - // - int nj = kdl_chain.getNrOfJoints(); - KDL::JntArray chain_min(nj), chain_max(nj); - - jointNames.clear(); - jointNames.reserve(nj); - - size_t seg_i, jnt_i; - for (seg_i = 0,jnt_i = 0; seg_i < kdl_chain.getNrOfSegments(); seg_i++) - { - const KDL::Segment & seg = kdl_chain.getSegment(seg_i); - if( seg.getJoint().getType() != KDL::Joint::None ) - { - std::string jnt_name = seg.getJoint().getName(); - // std::cerr << "searching for joint " << jnt_name << std::endl; - int tree_jnt = 0; - for(tree_jnt = 0; tree_jnt < joint_names.size(); tree_jnt++ ) - { - //std::cerr << "joint_names[ " << tree_jnt << "] is " << joint_names[tree_jnt] << std::endl; - if( joint_names[tree_jnt] == jnt_name ) - { - chain_min(jnt_i) = min(tree_jnt); - chain_max(jnt_i) = max(tree_jnt); - jnt_i++; - jointNames.push_back(jnt_name); - break; - } - } - if( tree_jnt == joint_names.size() ) - { - std::cerr << "Failure in converting limits from tree to chain, unable to find joint " << jnt_name << std::endl; - return false; - } - } - } - - if (jnt_i != nj) - { - std::cerr << "Failure in converting limits from tree to chain" << std::endl; - return false; - } - - // - // Convert the chain and the limits to an iKin chain (i.e. DH parameters) - // - result = iDynTree::iKinLimbFromKDLChain(kdl_chain,convertedChain,chain_min,chain_max); - if (!result) - { - std::cerr << "Could not export KDL::Tree to iKinChain" << std::endl; - return false; - } - return true; - } } diff --git a/toolbox/src/Jacobian.cpp b/toolbox/src/Jacobian.cpp index eefcc8d1c..c7574974c 100644 --- a/toolbox/src/Jacobian.cpp +++ b/toolbox/src/Jacobian.cpp @@ -1,161 +1,200 @@ #include "Jacobian.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" +#include "RobotInterface.h" #include "BlockInformation.h" #include "Signal.h" -#include -#include +#include +#include +#include #include -namespace wbt { +using namespace wbt; - std::string Jacobian::ClassName = "Jacobian"; +const std::string Jacobian::ClassName = "Jacobian"; - Jacobian::Jacobian() - : m_basePose(0) - , m_jacobian(0) - , m_basePoseRaw(0) - , m_configuration(0) - , m_frameIndex(-1) {} +const unsigned Jacobian::INPUT_IDX_BASE_POSE = 0; +const unsigned Jacobian::INPUT_IDX_JOINTCONF = 1; +const unsigned Jacobian::OUTPUT_IDX_FW_FRAME = 0; - unsigned Jacobian::numberOfParameters() - { - return WBIBlock::numberOfParameters() + 1; - } +Jacobian::Jacobian() +: m_frameIsCoM(false) +, m_frameIndex(iDynTree::FRAME_INVALID_INDEX) +{} - bool Jacobian::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } +unsigned Jacobian::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); +bool Jacobian::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - if (!blockInfo->setNumberOfInputPorts(2)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); - success = success && blockInfo->setInputPortVectorSize(1, dofs); + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // 3) Base frame velocity (1x6 vector) + // - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // Output port: - // - (6)x(6+dofs) matrix - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - success = blockInfo->setOutputPortMatrixSize(0, 6, 6 + dofs); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - return success; + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; } - bool Jacobian::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; + // OUTPUTS + // ======= + // + // 1) Matrix representing the Jacobian (6x(DoFs+6)) + // - int parentParameters = WBIBlock::numberOfParameters() + 1; - //robot name - std::string frame; - if (!blockInfo->getStringParameterAtIndex(parentParameters, frame)) { - if (error) error->message = "Cannot retrieve string from frame parameter"; - return false; - } + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } - //here obtain joint list and get the frame - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (!interface) { - if (error) error->message = "Cannot retrieve handle to WBI interface"; - return false; - } - wbi::IDList frames = interface->getFrameList(); - if (frame != "com") { - if (!frames.idToIndex(wbi::ID(frame), m_frameIndex)) { - if (error) error->message = "Cannot find " + frame + " in the frame list"; - return false; - } - } else { - m_frameIndex = wbi::wholeBodyInterface::COM_LINK_ID; - } + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_FW_FRAME, 6, 6 + dofs); + blockInfo->setOutputPortType(OUTPUT_IDX_FW_FRAME, PortDataTypeDouble); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_jacobian = new double[6 * (6 + dofs)]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; + return success; +} - return m_basePose && m_jacobian && m_basePoseRaw && m_configuration; +bool Jacobian::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + // INPUT PARAMETERS + // ================ + + std::string frame; + int parentParameters = WBBlock::numberOfParameters(); + + if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + Log::getSingleton().error("Cannot retrieve string from frame parameter."); + return false; } - bool Jacobian::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_jacobian) { - delete [] m_jacobian; - m_jacobian = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; + // Check if the frame is valid + // --------------------------- + + const auto& model = getRobotInterface()->getKinDynComputations(); + if (!model) { + Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); + return false; + } + + if (frame != "com") { + m_frameIndex = model->getFrameIndex(frame); + if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { + Log::getSingleton().error("Cannot find " + frame + " in the frame list."); + return false; } - return WBIModelBlock::terminate(blockInfo, error); + } + else { + m_frameIsCoM = true; + m_frameIndex = iDynTree::FRAME_INVALID_INDEX; } - bool Jacobian::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } + // OUTPUT / VARIABLES + // ================== - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); + m_jacobianCOM = std::unique_ptr(new iDynTree::MatrixDynSize(3, 6 + dofs)); + m_jacobianCOM->zero(); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - interface->computeJacobian(m_configuration, frame, m_frameIndex, m_jacobian); + // Output + m_jacobian = std::unique_ptr(new iDynTree::MatrixDynSize(6, 6 + dofs)); + m_jacobian->zero(); - Eigen::Map > jacobianRowMajor(m_jacobian, 6, 6 + dofs); + return static_cast(m_jacobianCOM) && static_cast(m_jacobian); +} - Signal output = blockInfo->getOutputPortSignal(0); - Eigen::Map > jacobianColMajor((double*)output.getContiguousBuffer(), 6, 6 + dofs); +bool Jacobian::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} - jacobianColMajor = jacobianRowMajor; - return true; - } +bool Jacobian::output(const BlockInformation* blockInfo) +{ + using namespace Eigen; + typedef Matrix MatrixXdSimulink; + typedef Matrix MatrixXdiDynTree; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== + + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + nullptr, + nullptr); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); return false; } + + // OUTPUT + // ====== + + iDynTree::Transform world_H_frame; + + // Compute the jacobian + ok = false; + if (!m_frameIsCoM) { + world_H_frame = model->getWorldTransform(m_frameIndex); + ok = model->getFrameFreeFloatingJacobian(m_frameIndex, *m_jacobian); + } + else { + world_H_frame.setPosition(model->getCenterOfMassPosition()); + ok = model->getCenterOfMassJacobian(*m_jacobianCOM); + int cols = m_jacobianCOM->cols(); + toEigen(*m_jacobian).block(0,0,3,cols) = toEigen(*m_jacobianCOM); + toEigen(*m_jacobian).block(3,0,3,cols).setZero(); + } + + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_FW_FRAME); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Allocate objects for row-major -> col-major conversion + Map jacobianRowMajor = toEigen(*m_jacobian); + Map jacobianColMajor(output.getBuffer(), + 6, 6 + dofs); + + // Forward the buffer to Simulink transforming it to ColMajor + jacobianColMajor = jacobianRowMajor; + return true; } diff --git a/toolbox/src/MassMatrix.cpp b/toolbox/src/MassMatrix.cpp index 2644213ee..6a6aa50fc 100644 --- a/toolbox/src/MassMatrix.cpp +++ b/toolbox/src/MassMatrix.cpp @@ -2,130 +2,142 @@ #include "BlockInformation.h" #include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include #include -namespace wbt { +using namespace wbt; - std::string MassMatrix::ClassName = "MassMatrix"; +const std::string MassMatrix::ClassName = "MassMatrix"; - MassMatrix::MassMatrix() - : m_basePose(0) - , m_massMatrix(0) - , m_basePoseRaw(0) - , m_configuration(0) {} +const unsigned MassMatrix::INPUT_IDX_BASE_POSE = 0; +const unsigned MassMatrix::INPUT_IDX_JOINTCONF = 1; +const unsigned MassMatrix::OUTPUT_IDX_MASS_MAT = 0; - bool MassMatrix::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } +MassMatrix::MassMatrix() {} - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); +bool MassMatrix::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here - // Specify I/O - // Input ports: - // - 4x4 matrix (homogenous transformation for the base pose w.r.t. world) - // - DoFs vector for the robot (joints) configurations + if (!WBBlock::configureSizeAndPorts(blockInfo)) { + return false; + } - if (!blockInfo->setNumberOfInputPorts(2)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - bool success = true; - success = success && blockInfo->setInputPortMatrixSize(0, 4, 4); - success = success && blockInfo->setInputPortVectorSize(1, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); - blockInfo->setInputPortType(1, PortDataTypeDouble); + // INPUTS + // ====== + // + // 1) Homogeneous transform for base pose wrt the world frame (4x4 matrix) + // 2) Joints position (1xDoFs vector) + // + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(2)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // Output port: - // - (DoFs + 6)x(DoFs + 6) matrix representing the mass matrix - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } + // Size and type + bool success = true; + success = success && blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4); + success = success && blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs); - success = blockInfo->setOutputPortMatrixSize(0, dofs + 6, dofs + 6); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_BASE_POSE, PortDataTypeDouble); + blockInfo->setInputPortType(INPUT_IDX_JOINTCONF, PortDataTypeDouble); - return success; + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; } - bool MassMatrix::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; - - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - m_basePose = new double[16]; - m_massMatrix = new double[(6 + dofs)*(6 + dofs)]; - m_basePoseRaw = new double[16]; - m_configuration = new double[dofs]; + // OUTPUTS + // ======= + // + // 1) Matrix epresenting the mass matrix (DoFs+6)x(DoFs+6) + // - return m_basePose && m_massMatrix && m_basePoseRaw && m_configuration; + // Number of outputs + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } - bool MassMatrix::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_basePose) { - delete [] m_basePose; - m_basePose = 0; - } - if (m_massMatrix) { - delete [] m_massMatrix; - m_massMatrix = 0; - } - if (m_basePoseRaw) { - delete [] m_basePoseRaw; - m_basePoseRaw = 0; - } - if (m_configuration) { - delete [] m_configuration; - m_configuration = 0; - } - return WBIModelBlock::terminate(blockInfo, error); + // Size and type + success = blockInfo->setOutputPortMatrixSize(OUTPUT_IDX_MASS_MAT, dofs + 6, dofs + 6); + blockInfo->setOutputPortType(OUTPUT_IDX_MASS_MAT, PortDataTypeDouble); + + return success; +} + +bool MassMatrix::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Output + m_massMatrix = std::unique_ptr(new iDynTree::MatrixDynSize(6 + dofs, 6 + dofs)); + m_massMatrix->zero(); + + return static_cast(m_massMatrix); +} + +bool MassMatrix::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} + +bool MassMatrix::output(const BlockInformation* blockInfo) +{ + using namespace Eigen; + using namespace iDynTree; + typedef Matrix MatrixXdSimulink; + typedef Matrix MatrixXdiDynTree; + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; } - bool MassMatrix::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - //get input - wbi::iWholeBodyModel * const interface = WBInterface::sharedInstance().model(); - if (interface) { - Signal basePoseRaw = blockInfo->getInputPortSignal(0); - Signal configuration = blockInfo->getInputPortSignal(1); - - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_basePoseRaw[i] = basePoseRaw.get(i).doubleData(); - } - for (unsigned i = 0; i < blockInfo->getInputPortWidth(1); ++i) { - m_configuration[i] = configuration.get(i).doubleData(); - } - - Eigen::Map > basePoseColMajor(m_basePoseRaw); - Eigen::Map > basePose(m_basePose); - basePose = basePoseColMajor; - - wbi::Frame frame; - wbi::frameFromSerialization(basePose.data(), frame); - - interface->computeMassMatrix(m_configuration, frame, m_massMatrix); - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - Eigen::Map > massMatrixRowMajor(m_massMatrix, 6 + dofs, 6 + dofs); - - Signal output = blockInfo->getOutputPortSignal(0); - Eigen::Map > massMatrixColMajor((double*)output.getContiguousBuffer(), 6 + dofs, 6 + dofs); - massMatrixColMajor = massMatrixRowMajor; - return true; - } + // GET THE SIGNALS POPULATE THE ROBOT STATE + // ======================================== + + Signal basePoseSig = blockInfo->getInputPortSignal(INPUT_IDX_BASE_POSE); + Signal jointsPosSig = blockInfo->getInputPortSignal(INPUT_IDX_JOINTCONF); + + bool ok = setRobotState(&basePoseSig, + &jointsPosSig, + nullptr, + nullptr); + + if (!ok) { + Log::getSingleton().error("Failed to set the robot state."); return false; } + + // OUTPUT + // ====== + + // Compute the Mass Matrix + model->getFreeFloatingMassMatrix(*m_massMatrix); + + // Get the output signal memory location + Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_MASS_MAT); + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + + // Allocate objects for row-major -> col-major conversion + Map massMatrixRowMajor = toEigen(*m_massMatrix); + Map massMatrixColMajor(output.getBuffer(), + 6 + dofs, 6 + dofs); + + // Forward the buffer to Simulink transforming it to ColMajor + massMatrixColMajor = massMatrixRowMajor; + return true; } diff --git a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp index 65832b2bd..05aa56527 100644 --- a/toolbox/src/MinimumJerkTrajectoryGenerator.cpp +++ b/toolbox/src/MinimumJerkTrajectoryGenerator.cpp @@ -1,6 +1,6 @@ #include "MinimumJerkTrajectoryGenerator.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" @@ -13,198 +13,234 @@ #include -#define PARAM_IDX_1 1 //Sample Time (double) -#define PARAM_IDX_2 2 //Settling Time (double) -#define PARAM_IDX_3 3 //Output first derivative (boolean) -#define PARAM_IDX_4 4 //Output second derivative (boolean) -#define PARAM_IDX_5 5 //Initial signal value as input (boolean) -#define PARAM_IDX_6 6 //Control if the settling time comes from external port or static parameter -#define PARAM_IDX_7 7 //True if the block should reset the traj generator in case settling time changes - -namespace wbt { - - std::string MinimumJerkTrajectoryGenerator::ClassName = "MinimumJerkTrajectoryGenerator"; - - MinimumJerkTrajectoryGenerator::MinimumJerkTrajectoryGenerator() - : m_generator(0) - , m_outputFirstDerivativeIndex(-1) - , m_outputSecondDerivativeIndex(-1) - , m_firstRun(true) - , m_explicitInitialValue(false) - , m_externalSettlingTime(false) - , m_resetOnSettlingTimeChange(false) - , m_initialValues(0) - , m_reference(0) {} - - unsigned MinimumJerkTrajectoryGenerator::numberOfParameters() { return 7; } - - bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - bool outputFirstDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); - bool outputSecondDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); - bool explicitInitialValue = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); - bool externalSettlingTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_6).booleanData(); - - int numberOfInputPorts = 1; - if (explicitInitialValue) - numberOfInputPorts++; - if (externalSettlingTime) - numberOfInputPorts++; - - // Specify I/O - // INPUTS - if(!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { - if (error) error->message = "Failed to set input port number"; - return false; - } +using namespace wbt; + +const std::string MinimumJerkTrajectoryGenerator::ClassName = "MinimumJerkTrajectoryGenerator"; + +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SAMPLE_TIME = 1; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_SETTLING_TIME = 2; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_1ST_DERIVATIVE = 3; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_OUTPUT_2ND_DERIVATIVE = 4; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_INITIAL_VALUE = 5; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_EXT_SETTLINGTIME = 6; +const unsigned MinimumJerkTrajectoryGenerator::PARAM_IDX_RESET_CHANGEST = 7; + +MinimumJerkTrajectoryGenerator::MinimumJerkTrajectoryGenerator() +: m_outputFirstDerivativeIndex(-1) +, m_outputSecondDerivativeIndex(-1) +, m_firstRun(true) +, m_explicitInitialValue(false) +, m_externalSettlingTime(false) +, m_resetOnSettlingTimeChange(false) +{} + +unsigned MinimumJerkTrajectoryGenerator::numberOfParameters() { return 7; } + +bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // PARAMETERS + // ========== + // + // 1) Sample time (double) + // 2) Settling time (double) + // 3) Enable the output of 1st derivative (bool) + // 4) Enable the output of 2nd derivative (bool) + // 5) Enable the input with the initial conditions (bool) + // 6) Enable the input with the external settling time (bool) + // 7) Reset the trajectory generator when settling time changes (bool) + // + + bool outputFirstDerivative; + bool outputSecondDerivative; + bool explicitInitialValue; + bool externalSettlingTime; + bool ok = true; + + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, outputFirstDerivative); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, outputSecondDerivative); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, explicitInitialValue); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, externalSettlingTime); + + if (!ok) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } - blockInfo->setInputPortVectorSize(0, -1); - blockInfo->setInputPortType(0, PortDataTypeDouble); + int numberOfInputPorts = 1; + numberOfInputPorts += static_cast(explicitInitialValue); + numberOfInputPorts += static_cast(externalSettlingTime); + + // INPUTS + // ====== + // + // 1) The reference signal (1xn) + // 2) The optional initial conditions + // 3) The optional settling time + // + + if(!blockInfo->setNumberOfInputPorts(numberOfInputPorts)) { + Log::getSingleton().error("Failed to set input port number."); + return false; + } - unsigned portIndex = 1; + blockInfo->setInputPortVectorSize(0, -1); + blockInfo->setInputPortType(0, PortDataTypeDouble); - if (explicitInitialValue) { - blockInfo->setInputPortVectorSize(portIndex, -1); - blockInfo->setInputPortType(portIndex, PortDataTypeDouble); - portIndex++; - } + unsigned portIndex = 1; - if (externalSettlingTime) - { - blockInfo->setInputPortVectorSize(portIndex, 1); - blockInfo->setInputPortType(portIndex, PortDataTypeDouble); - portIndex++; - } + if (explicitInitialValue) { + blockInfo->setInputPortVectorSize(portIndex, -1); + blockInfo->setInputPortType(portIndex, PortDataTypeDouble); + portIndex++; + } - // OUTPUTS - int numberOfOutputPorts = 1; - if (outputFirstDerivative) numberOfOutputPorts++; - if (outputSecondDerivative) numberOfOutputPorts++; + if (externalSettlingTime) { + blockInfo->setInputPortVectorSize(portIndex, 1); + blockInfo->setInputPortType(portIndex, PortDataTypeDouble); + portIndex++; + } - if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { - if (error) error->message = "Failed to set output port number"; - return false; - } + // OUTPUTS + // ======= + // + // 1) The calculated trajectory + // 2) The optional 1st derivative + // 3) The optional 2nd derivative + // + + int numberOfOutputPorts = 1; + numberOfOutputPorts += static_cast(outputFirstDerivative); + numberOfOutputPorts += static_cast(outputSecondDerivative); + + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } - for (int i = 0; i < numberOfOutputPorts; ++i) { - blockInfo->setOutputPortVectorSize(i, -1); - blockInfo->setOutputPortType(i, PortDataTypeDouble); - } + for (int i = 0; i < numberOfOutputPorts; ++i) { + blockInfo->setOutputPortVectorSize(i, -1); + blockInfo->setOutputPortType(i, PortDataTypeDouble); + } - return true; + return true; +} + +bool MinimumJerkTrajectoryGenerator::initialize(const BlockInformation* blockInfo) +{ + // Get the additional parameters + bool outputFirstDerivative; + bool outputSecondDerivative; + bool ok = true; + + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_1ST_DERIVATIVE, + outputFirstDerivative); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_OUTPUT_2ND_DERIVATIVE, + outputSecondDerivative); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_INITIAL_VALUE, m_explicitInitialValue); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_EXT_SETTLINGTIME, m_externalSettlingTime); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_RESET_CHANGEST, + m_resetOnSettlingTimeChange); + + if (!ok) { + Log::getSingleton().error("Failed to get input parameters."); + return false; } - bool MinimumJerkTrajectoryGenerator::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - //Save parameters - bool outputFirstDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); - bool outputSecondDerivative = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); + if (outputFirstDerivative) { + m_outputFirstDerivativeIndex = 1; + } - if (outputFirstDerivative) m_outputFirstDerivativeIndex = 1; - if (outputSecondDerivative) { - m_outputSecondDerivativeIndex = outputFirstDerivative ? 2 : 1; - } + if (outputSecondDerivative) { + m_outputSecondDerivativeIndex = outputFirstDerivative ? 2 : 1; + } - double sampleTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).doubleData(); - double settlingTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_2).doubleData(); + double sampleTime = 0; + double settlingTime = 0; - m_explicitInitialValue = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); - m_externalSettlingTime = blockInfo->getScalarParameterAtIndex(PARAM_IDX_6).booleanData(); - m_resetOnSettlingTimeChange = blockInfo->getScalarParameterAtIndex(PARAM_IDX_7).booleanData(); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_SAMPLE_TIME, sampleTime); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_SETTLING_TIME, settlingTime); - unsigned size = blockInfo->getInputPortWidth(0); + unsigned signalSize = blockInfo->getInputPortWidth(0); - m_generator = new iCub::ctrl::minJerkTrajGen(size, sampleTime, settlingTime); - m_previousSettlingTime = settlingTime; - m_initialValues = new yarp::sig::Vector(size); - m_reference = new yarp::sig::Vector(size); - if (!m_generator || !m_initialValues || !m_reference) { - if (error) error->message = "Could not allocate memory for trajectory generator"; - return false; - } + m_generator = std::unique_ptr + (new iCub::ctrl::minJerkTrajGen(signalSize, sampleTime, settlingTime)); + m_previousSettlingTime = settlingTime; + m_initialValues = std::unique_ptr(new yarp::sig::Vector(signalSize)); + m_reference = std::unique_ptr(new yarp::sig::Vector(signalSize)); - m_firstRun = true; - return true; + if (!m_generator || !m_initialValues || !m_reference) { + Log::getSingleton().error("Could not allocate memory for trajectory generator."); + return false; } - bool MinimumJerkTrajectoryGenerator::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - if (m_generator) { - delete m_generator; - m_generator = 0; - } - if (m_initialValues) { - delete m_initialValues; - m_initialValues = 0; - } - if (m_reference) { - delete m_reference; - m_reference = 0; - } - return true; - } - - bool MinimumJerkTrajectoryGenerator::output(BlockInformation *blockInfo, wbt::Error *error) - { - if (!m_generator) return false; - - if (m_externalSettlingTime) { - unsigned portIndex = 1; - if (m_explicitInitialValue) portIndex++; - Signal externalTimePort = blockInfo->getInputPortSignal(portIndex); - double externalTime = externalTimePort.get(0).doubleData(); - - if (std::abs(m_previousSettlingTime - externalTime) > 1e-5) { - m_previousSettlingTime = externalTime; - - m_generator->setT(externalTime); - if (m_resetOnSettlingTimeChange && !m_firstRun) { - m_generator->init(m_generator->getPos()); - } + m_firstRun = true; + return true; +} + +bool MinimumJerkTrajectoryGenerator::terminate(const BlockInformation* blockInfo) +{ + return true; +} + +bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) +{ + if (!m_generator) return false; + + if (m_externalSettlingTime) { + unsigned portIndex = 1; + if (m_explicitInitialValue) portIndex++; + Signal externalTimePort = blockInfo->getInputPortSignal(portIndex); + double externalTime = externalTimePort.get(0); + + if (std::abs(m_previousSettlingTime - externalTime) > 1e-5) { + m_previousSettlingTime = externalTime; + + m_generator->setT(externalTime); + if (m_resetOnSettlingTimeChange && !m_firstRun) { + m_generator->init(m_generator->getPos()); } } + } - if (m_firstRun) { - m_firstRun = false; - Signal initialValues; - unsigned portIndex = 0; - if (m_explicitInitialValue) { - portIndex = 1; - } - initialValues = blockInfo->getInputPortSignal(portIndex); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(portIndex); ++i) { - (*m_initialValues)[i] = initialValues.get(i).doubleData(); - } - m_generator->init(*m_initialValues); + if (m_firstRun) { + m_firstRun = false; + unsigned portIndex = 0; + if (m_explicitInitialValue) { + portIndex = 1; } + Signal initialValues = blockInfo->getInputPortSignal(portIndex); + for (unsigned i = 0; i < blockInfo->getInputPortWidth(portIndex); ++i) { + (*m_initialValues)[i] = initialValues.get(i); + } + m_generator->init(*m_initialValues); + } - Signal references = blockInfo->getInputPortSignal(0); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - (*m_reference)[i] = references.get(i).doubleData(); - } - m_generator->computeNextValues(*m_reference); + Signal references = blockInfo->getInputPortSignal(0); + for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { + (*m_reference)[i] = references.get(i); + } + m_generator->computeNextValues(*m_reference); - const yarp::sig::Vector &signal = m_generator->getPos(); - Signal output = blockInfo->getOutputPortSignal(0); - output.setBuffer(signal.data(), signal.size()); + const yarp::sig::Vector &signal = m_generator->getPos(); + Signal output = blockInfo->getOutputPortSignal(0); + output.setBuffer(signal.data(), signal.size()); // for (unsigned i = 0; i < blockInfo->getOutputPortWidth(0); ++i) { // output.set(i, signal[i]); // } - //note: index of the port is not known a priori. - //I should save it in the initialization phase - if (m_outputFirstDerivativeIndex != -1) { - const yarp::sig::Vector &derivative = m_generator->getVel(); - Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputFirstDerivativeIndex); - derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputFirstDerivativeIndex)); - } - if (m_outputSecondDerivativeIndex != -1) { - const yarp::sig::Vector &derivative = m_generator->getAcc(); - Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputSecondDerivativeIndex); - derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputSecondDerivativeIndex)); - } + //note: index of the port is not known a priori. + //I should save it in the initialization phase + if (m_outputFirstDerivativeIndex != -1) { + const yarp::sig::Vector &derivative = m_generator->getVel(); + Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputFirstDerivativeIndex); + derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputFirstDerivativeIndex)); + } - return true; + if (m_outputSecondDerivativeIndex != -1) { + const yarp::sig::Vector &derivative = m_generator->getAcc(); + Signal derivativeOutput = blockInfo->getOutputPortSignal(m_outputSecondDerivativeIndex); + derivativeOutput.setBuffer(derivative.data(), blockInfo->getOutputPortWidth(m_outputSecondDerivativeIndex)); } + return true; } diff --git a/toolbox/src/ModelPartitioner.cpp b/toolbox/src/ModelPartitioner.cpp new file mode 100644 index 000000000..21f872c9c --- /dev/null +++ b/toolbox/src/ModelPartitioner.cpp @@ -0,0 +1,204 @@ +#include "ModelPartitioner.h" + +#include "BlockInformation.h" +#include "Configuration.h" +#include "RobotInterface.h" +#include "Signal.h" +#include "Log.h" +#include + +using namespace wbt; + +const std::string ModelPartitioner::ClassName = "ModelPartitioner"; + +unsigned ModelPartitioner::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 1; +} + +bool ModelPartitioner::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // PARAMETERS + // ========== + // + // 1) Boolean specifying if yarp2WBI (true) or WBI2yarp (false) + // + + bool yarp2WBI = false; + unsigned yarp2WBIParameterIdx = WBBlock::numberOfParameters() + 1; + + if (!blockInfo->getBooleanParameterAtIndex(yarp2WBIParameterIdx, yarp2WBI)) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } + + // INPUTS + // ====== + // + // yarp2WBI + // -------- + // + // 1) Vector containing the data vector (1 x DoFs) + // + // WBI2yarp + // -------- + // + // n signals) The n ControlBoards configured from the config block + // + + bool ok; + unsigned numberOfInputs; + unsigned controlBoardsNumber = getConfiguration().getControlBoardsNames().size(); + + if (yarp2WBI) { + numberOfInputs = 1; + ok = blockInfo->setNumberOfInputPorts(numberOfInputs); + blockInfo->setInputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); + blockInfo->setInputPortType(0, PortDataTypeDouble); + } + else { + numberOfInputs = controlBoardsNumber; + ok = blockInfo->setNumberOfInputPorts(numberOfInputs); + // Set the size as dynamic + for (unsigned i = 0; i < numberOfInputs; ++i) { + blockInfo->setInputPortVectorSize(i, -1); + blockInfo->setInputPortType(i, PortDataTypeDouble); + } + } + + if (!ok) { + Log::getSingleton().error("Failed to set input port number."); + return false; + } + + // OUTPUTS + // ======= + // + // yarp2WBI + // -------- + // + // n signals) The n ControlBoards configured from the config block + // + // WBI2yarp + // -------- + // + // 1) Vector containing the data vector (1 x DoFs) + // + + unsigned numberOfOutputs; + + if (yarp2WBI) { + numberOfOutputs = controlBoardsNumber; + ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); + // Set the size as dynamic + for (unsigned i = 0; i < numberOfOutputs; ++i) { + blockInfo->setOutputPortVectorSize(i, -1); + blockInfo->setOutputPortType(i, PortDataTypeDouble); + } + } + else { + numberOfOutputs = 1; + ok = blockInfo->setNumberOfOutputPorts(numberOfOutputs); + blockInfo->setOutputPortVectorSize(0, getConfiguration().getNumberOfDoFs()); + blockInfo->setOutputPortType(0, PortDataTypeDouble); + } + + // For some reason, the output ports widths in the yarp2WBI case are not detected + // properly by Simulink if set as DYNAMICALLY_SIZED (-1). + // Set them manually using the m_controlBoardIdxLimit map. + // + // Doing this now has the disadvantage of allocating the KinDynComputations and the + // RemoteControlBoardRemapper already at this early stage, but this happens only at the + // first execution of the model if the joint list doesn't change. + // + if (yarp2WBI) { + m_controlBoardIdxLimit = getRobotInterface()->getControlBoardIdxLimit(); + if (!m_controlBoardIdxLimit) { + Log::getSingleton().error("Failed to get the map CBIdx <--> CBMaxIdx."); + return false; + } + for (const auto& cb : *m_controlBoardIdxLimit) { + if (!blockInfo->setOutputPortVectorSize(cb.first, cb.second)) { + Log::getSingleton().error("Failed to set ouput port size reading them from cb map."); + return false; + } + } + } + + if (!ok) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } + + return true; +} + +bool ModelPartitioner::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + unsigned yarp2WBIParameterIdx = WBBlock::numberOfParameters() + 1; + if (!blockInfo->getBooleanParameterAtIndex(yarp2WBIParameterIdx, m_yarp2WBI)) { + Log::getSingleton().error("Failed to get input parameters."); + return false; + } + + m_jointsMapString = getRobotInterface()->getJointsMapString(); + m_controlledJointsMapCB = getRobotInterface()->getControlledJointsMapCB(); + + if (!m_jointsMapString) { + Log::getSingleton().error("Failed to get the joint map iDynTree <--> Yarp."); + return false; + } + + if (!m_controlledJointsMapCB) { + Log::getSingleton().error("Failed to get the joint map iDynTree <--> controlledJointsIdx."); + return false; + } + + return true; +} + +bool ModelPartitioner::terminate(const BlockInformation* blockInfo) +{ + return WBBlock::terminate(blockInfo); +} + +bool ModelPartitioner::output(const BlockInformation* blockInfo) +{ + if (m_yarp2WBI) { + Signal dofsSignal = blockInfo->getInputPortSignal(0); + + for (unsigned ithJoint = 0; ithJoint < getConfiguration().getNumberOfDoFs(); ++ithJoint) { + const std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + // Get the ControlBoard number the ith joint belongs + const cb_idx& controlBoardOfJoint = m_jointsMapString->at(ithJointName).first; + // Get the index of the ith joint inside the controlledJoints vector relative to + // its ControlBoard + const controlledJointIdxCB contrJointIdxCB = m_controlledJointsMapCB->at(ithJointName); + + // Get the data to forward + Signal ithOutput = blockInfo->getOutputPortSignal(controlBoardOfJoint); + ithOutput.set(contrJointIdxCB, dofsSignal.get(ithJoint)); + } + } + else { + Signal dofsSignal = blockInfo->getOutputPortSignal(0); + + for (unsigned ithJoint = 0; ithJoint < getConfiguration().getNumberOfDoFs(); ++ithJoint) { + const std::string ithJointName = getConfiguration().getControlledJoints()[ithJoint]; + // Get the ControlBoard number the ith joint belongs + const cb_idx& controlBoardOfJoint = m_jointsMapString->at(ithJointName).first; + // Get the index of the ith joint inside the controlledJoints vector relative to + // its ControlBoard + const controlledJointIdxCB contrJointIdxCB = m_controlledJointsMapCB->at(ithJointName); + + // Get the data to forward + const Signal ithInput = blockInfo->getInputPortSignal(controlBoardOfJoint); + dofsSignal.set(ithJoint, ithInput.get(contrJointIdxCB)); + } + } + return true; +} diff --git a/toolbox/src/RealTimeSynchronizer.cpp b/toolbox/src/RealTimeSynchronizer.cpp index 678350493..62701a38c 100644 --- a/toolbox/src/RealTimeSynchronizer.cpp +++ b/toolbox/src/RealTimeSynchronizer.cpp @@ -1,71 +1,89 @@ #include "RealTimeSynchronizer.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include -#define PARAM_IDX_1 1 // Period - -namespace wbt { - - std::string RealTimeSynchronizer::ClassName = "RealTimeSynchronizer"; - - RealTimeSynchronizer::RealTimeSynchronizer() - : m_period(0.01) - , m_initialTime(0) - , m_counter(0) {} - - RealTimeSynchronizer::~RealTimeSynchronizer() {} - - unsigned RealTimeSynchronizer::numberOfParameters() { return 1; } - - bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; - return false; - } - - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to set output port number"; - return false; - } - - return true; +using namespace wbt; + +const std::string RealTimeSynchronizer::ClassName = "RealTimeSynchronizer"; + +const unsigned RealTimeSynchronizer::PARAM_PERIOD = 1; // Period + +RealTimeSynchronizer::RealTimeSynchronizer() +: m_period(0.01) +, m_initialTime(0) +, m_counter(0) +{} + +unsigned RealTimeSynchronizer::numberOfParameters() { return 1; } + +bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } + + // OUTPUTS + // ======= + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); + return false; } - bool RealTimeSynchronizer::initialize(BlockInformation *blockInfo, wbt::Error */*error*/) - { - m_period = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).doubleData(); - m_counter = 0; - return m_period > 0; + return true; +} + +bool RealTimeSynchronizer::initialize(const BlockInformation* blockInfo) +{ + if (!blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period)) { + Log::getSingleton().error("Failed to get input parametes."); + return false; } - - bool RealTimeSynchronizer::terminate(BlockInformation *blockInfo, wbt::Error */*error*/) - { - return true; + + if (m_period < 0) { + Log::getSingleton().error("Period must be greater than 0."); + return false; } - - bool RealTimeSynchronizer::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - using namespace yarp::os; - if (m_counter == 0) { - m_initialTime = yarp::os::Time::now(); - } + m_counter = 0; + return true; +} - //read current time - double currentTime = yarp::os::Time::now() - m_initialTime; - double desiredTime = m_counter * m_period; +bool RealTimeSynchronizer::terminate(const BlockInformation* blockInfo) +{ + return true; +} - double sleepPeriod = desiredTime - currentTime; +bool RealTimeSynchronizer::output(const BlockInformation* blockInfo) +{ + if (m_counter == 0) { + m_initialTime = yarp::os::Time::now(); + } + + //read current time + double currentTime = yarp::os::Time::now() - m_initialTime; + double desiredTime = m_counter* m_period; - //sleep for the remaining time - if (sleepPeriod > 0) - yarp::os::Time::delay(sleepPeriod); - - m_counter++; + double sleepPeriod = desiredTime - currentTime; - return true; + //sleep for the remaining time + if (sleepPeriod > 0) { + yarp::os::Time::delay(sleepPeriod); } + + m_counter++; + + return true; } diff --git a/toolbox/src/SetLowLevelPID.cpp b/toolbox/src/SetLowLevelPID.cpp index b772cb742..911b039d8 100644 --- a/toolbox/src/SetLowLevelPID.cpp +++ b/toolbox/src/SetLowLevelPID.cpp @@ -1,275 +1,247 @@ #include "SetLowLevelPID.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" +#include "RobotInterface.h" #include "BlockInformation.h" -#include -#include -#include -#include -#include -#include - -namespace wbt { - - static const std::string TorquePIDInitialKey = "__ORIGINAL_PIDs__"; - static const std::string TorquePIDDefaultKey = "__DEFAULT_PIDs__"; - -#pragma mark - SetLowLevelPID class implementation - - std::string SetLowLevelPID::ClassName = "SetLowLevelPID"; - - SetLowLevelPID::SetLowLevelPID() - : m_firstRun(true) - , m_lastGainSetIndex(-1) - , m_controlMode(wbi::CTRL_MODE_TORQUE) {} - - - bool SetLowLevelPID::loadLowLevelGainsFromFile(std::string filename, - const yarpWbi::PIDList &originalList, - wbi::wholeBodyInterface& interface, - yarpWbi::PIDList &loadedPIDs) - { - //List of list. Each element has a key: joint name, and a list of pairs: kp, kd, ki and its respective value - using namespace yarp::os; - yarp::os::ResourceFinder resourceFinder = yarp::os::ResourceFinder::getResourceFinderSingleton(); - Property file; - std::string fileName = resourceFinder.findFileByName(filename); - if (fileName.empty()) return false; - file.fromConfigFile(fileName); - - Bottle externalList; - externalList.fromString(file.toString()); - - bool result = true; - wbi::IDList jointList = interface.getJointList(); - for (int i = 0; i < externalList.size(); ++i) { - if (!externalList.get(i).isList()) continue; - Bottle *jointConfig = externalList.get(i).asList(); - if (jointConfig->size() < 2 || !jointConfig->get(0).isString()) continue; - wbi::ID jointID(jointConfig->get(0).asString()); - int jointIndex = -1; - if (!jointList.idToIndex(jointID, jointIndex)) continue; - if (jointIndex < 0 || jointIndex >= jointList.size()) { - yWarning("Specified joint %s index is outside joint list size", jointID.toString().c_str()); - continue; - } - - loadedPIDs.pidList()[jointIndex] = originalList.pidList()[jointIndex]; - loadedPIDs.motorParametersList()[jointIndex] = originalList.motorParametersList()[jointIndex]; - - result = result && yarpWbi::pidFromBottleDescription(*jointConfig, loadedPIDs.pidList()[jointIndex]); - if (m_controlMode == wbi::CTRL_MODE_TORQUE) { - result = result && yarpWbi::motorTorqueParametersFromBottleDescription(*jointConfig, loadedPIDs.motorParametersList()[jointIndex]); - } - } - return result; - } +#include +#include - bool SetLowLevelPID::loadGainsFromValue(const yarp::os::Value &gains, - PidMap &pidMap, - wbi::wholeBodyInterface& interface) - { - pidMap.clear(); +using namespace wbt; - yarpWbi::yarpWholeBodyInterface *yarpInterface = dynamic_cast(&interface); - if (!yarpInterface) { - return false; - } +const std::string SetLowLevelPID::ClassName = "SetLowLevelPID"; - //Load original gains from controlboards and save them to the original key. - yarpWbi::PIDList originalGains(interface.getDoFs()); - yarpWbi::yarpWholeBodyActuators *actuators = yarpInterface->wholeBodyActuator(); - if (!actuators) { - return false; - } - //TODO: should be made not limited to torque - actuators->getPIDGains(originalGains.pidList(), wbi::CTRL_MODE_TORQUE); - actuators->getMotorTorqueParameters(originalGains.motorParametersList()); - pidMap.insert(PidMap::value_type(TorquePIDInitialKey, originalGains)); - - //Now load additional gains - bool result = true; - if (gains.isString()) { - yarpWbi::PIDList pids(interface.getDoFs()); - result = loadLowLevelGainsFromFile(gains.asString(), originalGains, interface, pids); - pidMap.insert(PidMap::value_type(TorquePIDDefaultKey, pids)); - } else if (gains.isList()) { - using namespace yarp::os; - Bottle *list = gains.asList(); - - //list of files. gains will be saved as integer-values - for (int i = 0; i < list->size(); ++i) { - if (!list->get(i).isString()) continue; - std::string filename = list->get(i).asString(); - yarpWbi::PIDList pids(interface.getDoFs()); - result = loadLowLevelGainsFromFile(filename, originalGains, interface, pids); - - if (result) { - pidMap.insert(PidMap::value_type(yarpWbi::stringFromInt(i + 1), pids)); - } - } - } - return result; +unsigned SetLowLevelPID::numberOfParameters() +{ + return WBBlock::numberOfParameters() + + 1 // WBTPIDConfig object + + 1; // Control type +} + +bool SetLowLevelPID::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; + + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; } - //TODO: for performance it is probably better to change the map so that the index is an integer - //i.e. a non continuous vector - bool SetLowLevelPID::setCurrentGains(const PidMap &pidMap, - std::string key, - wbi::iWholeBodyActuators& actuators) - { - yarpWbi::yarpWholeBodyActuators *yarpActuators = static_cast(&actuators); - if (!yarpActuators) return false; - - PidMap::const_iterator found = pidMap.find(key); - //Treat one exception: pidMap with size == 2, the default can be set to either the string or the num 1 - if (found == pidMap.end() && key == TorquePIDDefaultKey && pidMap.size() == 2) { - found = pidMap.find("1"); - } + // OUTPUTS + // ======= + // + // No outputs + // - if (found == pidMap.end()) return false; - bool result = yarpActuators->setPIDGains(found->second.pidList(), m_controlMode); //to be made generic (torque, position, etc) - if (m_controlMode == wbi::CTRL_MODE_TORQUE) { - result = result && yarpActuators->setMotorTorqueParameters(found->second.motorParametersList()); - } - return result; + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; } -#pragma mark - overloaded methods + return true; +} - unsigned SetLowLevelPID::numberOfParameters() - { - return WBIBlock::numberOfParameters() - + 1 // pid parameters file - + 1;// control method +bool SetLowLevelPID::readWBTPidConfigObject(const BlockInformation* blockInfo) +{ + AnyStruct s; + if (!blockInfo->getStructAtIndex(WBBlock::numberOfParameters() + 1, s)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; } - bool SetLowLevelPID::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + // Check the existence of all the fields + try { + s.at("P"); + s.at("I"); + s.at("D"); + s.at("jointList"); + } + catch (const std::out_of_range& e) { + Log::getSingleton().error("Cannot retrieve one or more parameter from parameter's struct."); + return false; + } - // Specify I/O - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } + // Proportional gains + std::vector Pvector; + if (!s["P"]->asVectorDouble(Pvector)) { + Log::getSingleton().error("Cannot retrieve vector from P parameter."); + return false; + } - // Output port: - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } + // Integral gains + std::vector Ivector; + if (!s["I"]->asVectorDouble(Ivector)) { + Log::getSingleton().error("Cannot retrieve vector from I parameter."); + return false; + } - return true; + // Derivative gains + std::vector Dvector; + if (!s["D"]->asVectorDouble(Dvector)) { + Log::getSingleton().error("Cannot retrieve vector from D parameter."); + return false; } - bool SetLowLevelPID::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; + // Considered joint names + AnyCell jointPidsCell; + if (!s["jointList"]->asAnyCell(jointPidsCell)) { + Log::getSingleton().error("Cannot retrieve string from jointList parameter."); + return false; + } - // Reading the control type - std::string controlType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 2, controlType)) { - if (error) error->message = "Could not read control type parameter"; + // From AnyCell to vector + std::vector jointNamesFromParameters; + for (auto cell: jointPidsCell) { + std::string joint; + if (!cell->asString(joint)) { + Log::getSingleton().error("Failed to convert jointList from cell to strings."); return false; } + jointNamesFromParameters.push_back(joint); + } - m_controlMode = wbi::CTRL_MODE_UNKNOWN; - if (controlType == "Position") { - m_controlMode = wbi::CTRL_MODE_POS; - } else if (controlType == "Torque") { - m_controlMode = wbi::CTRL_MODE_TORQUE; - } else { - if (error) error->message = "Control Mode not supported"; - return false; + if (Pvector.size() != Ivector.size() || + Ivector.size() != Dvector.size() || + Dvector.size() != jointNamesFromParameters.size()) { + Log::getSingleton().error("Sizes of P, I, D, and jointList elements are not the same."); + return false; + } + + // Store this data into a private member map + for (unsigned i = 0; i < jointNamesFromParameters.size(); ++i) { + // Check the processed joint is actually a controlledJoint + const auto& controlledJoints = getConfiguration().getControlledJoints(); + auto findElement = std::find(std::begin(controlledJoints), + std::end(controlledJoints), + jointNamesFromParameters[i]); + if (findElement != std::end(controlledJoints)) { + m_pidJointsFromParameters[jointNamesFromParameters[i]] = {Pvector[i], Ivector[i], Dvector[i]}; } + else { + Log::getSingleton().warning("Attempted to set PID of joint " + jointNamesFromParameters[i]); + Log::getSingleton().warningAppend(" non currently controlled. Skipping it."); + } + + } + if (m_pidJointsFromParameters.size() != jointNamesFromParameters.size()) { + Log::getSingleton().warning("PID have been passed only for a subset of the controlled joints."); + } - // Reading the PID specification parameter - std::string pidParameter; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, pidParameter)) { - if (error) error->message = "Could not read PID file specification parameter"; - return false; - } + return true; +} - Value value; - value.fromString(pidParameter.c_str()); - m_pids.clear(); +bool SetLowLevelPID::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; - yarpWbi::yarpWholeBodyInterface * const interface = dynamic_cast(WBInterface::sharedInstance().interface()); - if (!interface) { - if (error) error->message = "This block currently work only with YARP-WBI implementation"; - return false; - } - if (!loadGainsFromValue(value, m_pids, *interface)) { - m_pids.clear(); - if (error) error->message = "Error while loading PIDs configuration"; - yError("Error while loading PIDs configuration"); - return false; - } else { - yInfo("Loaded PIDs configuration"); - } - m_lastGainSetIndex = -1; + // Reading the control type + std::string controlType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 2, controlType)) { + Log::getSingleton().error("Could not read control type parameter."); + return false; + } - m_firstRun = true; - return true; + if (controlType == "Position") { + m_controlType = yarp::dev::VOCAB_PIDTYPE_POSITION; + } + else if (controlType == "Torque") { + m_controlType = yarp::dev::VOCAB_PIDTYPE_TORQUE; + } + else { + Log::getSingleton().error("Control type not recognized."); + return false; } - bool SetLowLevelPID::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - //static_cast as the dynamic has been done in the initialize - //and the pointer should not change - yarpWbi::yarpWholeBodyInterface * const interface = static_cast(WBInterface::sharedInstance().interface()); - - if (interface) { - yarpWbi::yarpWholeBodyActuators *actuators = interface->wholeBodyActuator(); - if (actuators && m_pids.size() > 1) { - setCurrentGains(m_pids, TorquePIDInitialKey, *actuators); - } - } + // Reading the WBTPIDConfig matlab class + if (!readWBTPidConfigObject(blockInfo)) { + Log::getSingleton().error("Failed to parse the WBTPIDConfig object."); + return false; + } + + // Retain the RemoteControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); + return false; + } - m_pids.clear(); - m_lastGainSetIndex = -1; + const unsigned& dofs = getConfiguration().getNumberOfDoFs(); - return WBIBlock::terminate(blockInfo, error); + // Initialize the vector size to the number of dofs + m_defaultPidValues.resize(dofs); + + // Get the interface + yarp::dev::IPidControl* iPidControl = nullptr; + if (!getRobotInterface()->getInterface(iPidControl) || !iPidControl) { + Log::getSingleton().error("Failed to get IPidControl interface."); + return false; + } + + // Store the default gains + if (!iPidControl->getPids(m_controlType, m_defaultPidValues.data())) { + Log::getSingleton().error("Failed to get default data from IPidControl."); + return false; } - bool SetLowLevelPID::output(BlockInformation *blockInfo, wbt::Error *error) - { - //static_cast as the dynamic has been done in the initialize - //and the pointer should not change - yarpWbi::yarpWholeBodyInterface * const interface = static_cast(WBInterface::sharedInstance().interface()); - if (interface) { - if (m_firstRun) { - m_firstRun = false; - - yarpWbi::yarpWholeBodyActuators *actuators = interface->wholeBodyActuator(); - if (!actuators) { - if (error) error->message = "Failed to retrieve the interface to the actuators"; - return false; - } - - //First case: only one element - if (m_lastGainSetIndex == -1 && m_pids.size() == 2) { - //just switch to the only existing set - setCurrentGains(m_pids, TorquePIDDefaultKey, *actuators); - m_lastGainSetIndex = 0; - } else { -// InputPtrsType u = ssGetInputPortSignalPtrs(S, 0); -// InputInt8PtrsType uPtrs = (InputInt8PtrsType)u; -// if (*uPtrs[0] != lastGainIndex) { -// wbitoolbox::setCurrentGains(*pids, *uPtrs[0], *((yarpWbi::yarpWholeBodyInterface*)robot->wbInterface)); -// info[0] = *uPtrs[0]; -// } - } - - } - return true; + // Initialize the vector of the applied pid gains with the default gains + m_appliedPidValues = m_defaultPidValues; + + // Override the PID with the gains specified as block parameters + for (unsigned i = 0; i < dofs; ++i) { + const std::string jointName = getConfiguration().getControlledJoints()[i]; + // If the pid has been passed, set the new gains + if (m_pidJointsFromParameters.find(jointName) != m_pidJointsFromParameters.end()) { + PidData gains = m_pidJointsFromParameters[jointName]; + m_appliedPidValues[i].setKp(std::get(gains)); + m_appliedPidValues[i].setKi(std::get(gains)); + m_appliedPidValues[i].setKd(std::get(gains)); } + } + + // Apply the new pid gains + if (!iPidControl->setPids(m_controlType, m_appliedPidValues.data())) { + Log::getSingleton().error("Failed to set PID values."); return false; } + + return true; +} + +bool SetLowLevelPID::terminate(const BlockInformation* blockInfo) +{ + bool ok = true; + + // Get the IPidControl interface + yarp::dev::IPidControl* iPidControl = nullptr; + ok = ok && getRobotInterface()->getInterface(iPidControl); + if (!ok || !iPidControl) { + Log::getSingleton().error("Failed to get IPidControl interface."); + // Don't return false here. WBBlock::terminate must be called in any case + } + + // Reset default pid gains + ok = ok && iPidControl->setPids(m_controlType, m_defaultPidValues.data()); + if (!ok) { + Log::getSingleton().error("Failed to set default PID values."); + // Don't return false here. WBBlock::terminate must be called in any case + } + + // Release the RemoteControlBoardRemapper + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case + } + + return ok && WBBlock::terminate(blockInfo); +} + +bool SetLowLevelPID::output(const BlockInformation* blockInfo) +{ + return true; } diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 8a8f4464e..e8681fc33 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -1,201 +1,323 @@ #include "SetReferences.h" -#include "Error.h" -#include "WBInterface.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" -#include -#include +#include "RobotInterface.h" +#include -namespace wbt { +#define _USE_MATH_DEFINES +#include - std::string SetReferences::ClassName = "SetReferences"; +using namespace wbt; - SetReferences::SetReferences() - : m_references(0) - , m_controlMode(wbi::CTRL_MODE_UNKNOWN) - , m_fullControl(true) - , m_resetControlMode(true) {} +const std::string SetReferences::ClassName = "SetReferences"; - unsigned SetReferences::numberOfParameters() - { - // 1 - Control Type - // 2 - Full/Sublist type - // 3 - (only if sublist) controlled joints - return WBIBlock::numberOfParameters() + 3; +SetReferences::SetReferences() +: m_resetControlMode(true) +, m_refSpeed(0) +{} + +const std::vector SetReferences::rad2deg(const double* buffer, const unsigned width) +{ + const double Rad2Deg = 180.0 / M_PI; + + std::vector vectorDeg(width); + + for (auto i = 0; i < width; ++i) { + vectorDeg[i] = buffer[i] * Rad2Deg; } - bool SetReferences::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } + return vectorDeg; +} - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); +unsigned SetReferences::numberOfParameters() +{ + return WBBlock::numberOfParameters() + 2; +} - m_fullControl = blockInfo->getScalarParameterAtIndex(WBIBlock::numberOfParameters() + 2).booleanData(); +bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Memory allocation / Saving data not allowed here - if (!m_fullControl) { - //sublist - std::string controlledList; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 3, controlledList)) { - if (error) error->message = "Could not read control type parameter"; - return false; - } - const yarp::os::Property * controlledListProperty = WBInterface::sharedInstance().currentConfiguration(); + if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - wbi::IDList idList; - WBInterface::wbdIDListFromConfigPropAndList(*controlledListProperty, - controlledList, idList); - dofs = idList.size(); - } + const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // Specify I/O - if (!blockInfo->setNumberOfInputPorts(1)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } + // INPUTS + // ====== + // + // 1) Joint refereces (1xDoFs vector) + // + + // Number of inputs + if (!blockInfo->setNumberOfInputPorts(1)) { + Log::getSingleton().error("Failed to configure the number of input ports."); + return false; + } + + // Size and type + bool success = blockInfo->setInputPortVectorSize(0, dofs); + blockInfo->setInputPortType(0, PortDataTypeDouble); + + if (!success) { + Log::getSingleton().error("Failed to configure input ports."); + return false; + } + + // OUTPUTS + // ======= + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to configure the number of output ports."); + return false; + } + + return true; +} + +bool SetReferences::initialize(const BlockInformation* blockInfo) +{ + if (!WBBlock::initialize(blockInfo)) return false; + + // Reading the control type + std::string controlType; + if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, + controlType)) { + Log::getSingleton().error("Could not read control type parameter."); + return false; + } + + // Reading the refSpeed + if (!blockInfo->getScalarParameterAtIndex(WBBlock::numberOfParameters() + 2, + m_refSpeed)) { + Log::getSingleton().error("Could not read reference speed parameter."); + return false; + } - bool success = blockInfo->setInputPortVectorSize(0, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); + // Initialize the std::vectors + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_controlModes.assign(dofs, VOCAB_CM_UNKNOWN); - if (!success) { - if (error) error->message = "Failed to configure input ports"; + // IControlMode.h + if (controlType == "Position") { + m_controlModes.assign(dofs, VOCAB_CM_POSITION); + } + else if (controlType == "Position Direct") { + m_controlModes.assign(dofs, VOCAB_CM_POSITION_DIRECT); + } + else if (controlType == "Velocity") { + m_controlModes.assign(dofs, VOCAB_CM_VELOCITY); + } + else if (controlType == "Torque") { + m_controlModes.assign(dofs, VOCAB_CM_TORQUE); + } + else if (controlType == "PWM") { + m_controlModes.assign(dofs, VOCAB_CM_PWM); + } + else if (controlType == "Current") { + m_controlModes.assign(dofs, VOCAB_CM_CURRENT); + } + else { + Log::getSingleton().error("Control Mode not supported."); + return false; + } + + // The Position mode is used to set a discrete reference, and then the yarp interface + // is responsible to generate a trajectory to reach this setpoint. + // The generated trajectory takes an additional parameter: the speed. + // If not properly initialized, this contol mode does not work as expected. + if (controlType == "Position") { + // Get the interface + yarp::dev::IPositionControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get IPositionControl interface."); return false; } - - // Output port: - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to configure the number of output ports"; + std::vector speedInitalization(dofs, m_refSpeed); + // Set the references + if (!interface->setRefSpeeds(speedInitalization.data())) { + Log::getSingleton().error("Failed to initialize speed references."); return false; } + } + + // Retain the ControlBoardRemapper + if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); + return false; + } + + m_resetControlMode = true; + return true; +} + +bool SetReferences::terminate(const BlockInformation* blockInfo) +{ + using namespace yarp::dev; + bool ok = true; + + // Get the interface + IControlMode2* icmd2 = nullptr; + ok = ok && getRobotInterface()->getInterface(icmd2); + if (!ok || !icmd2) { + Log::getSingleton().error("Failed to get the IControlMode2 interface."); + // Don't return false here. WBBlock::terminate must be called in any case + } + + // Set all the controlledJoints VOCAB_CM_POSITION + const unsigned dofs = getConfiguration().getNumberOfDoFs(); + m_controlModes.assign(dofs, VOCAB_CM_POSITION); - return true; + ok = ok && icmd2->setControlModes(m_controlModes.data()); + if (!ok) { + Log::getSingleton().error("Failed to set control mode."); + // Don't return false here. WBBlock::terminate must be called in any case } - bool SetReferences::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - if (!WBIBlock::initialize(blockInfo, error)) return false; + // Release the RemoteControlBoardRemapper + ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); + if (!ok) { + Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); + // Don't return false here. WBBlock::terminate must be called in any case + } + + return ok && WBBlock::terminate(blockInfo); +} - // Reading the control type - std::string controlType; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 1, controlType)) { - if (error) error->message = "Could not read control type parameter"; +bool SetReferences::initializeInitialConditions(const BlockInformation* /*blockInfo*/) +{ + // This function is called when a subsystem with Enable / Disable support is used. + // In this case all the blocks in this subsystem are configured and initialize, + // but if they are disabled, output() is not called. + // This initializeInitialConditions method is called when the block is enabled, + // and in this case the control mode should be set. + // + // It is worth noting that this toolbox disables parameters to be tunable for + // all the blocks + + m_resetControlMode = true; + return true; +} + +bool SetReferences::output(const BlockInformation* blockInfo) +{ + using namespace yarp::dev; + + // Set the control mode at the first run + if (m_resetControlMode) { + m_resetControlMode = false; + // Get the interface + IControlMode2* icmd2 = nullptr; + if (!getRobotInterface()->getInterface(icmd2) || !icmd2) { + Log::getSingleton().error("Failed to get the IControlMode2 interface."); return false; } - - m_controlMode = wbi::CTRL_MODE_UNKNOWN; - if (controlType == "Position") { - m_controlMode = wbi::CTRL_MODE_POS; - } else if (controlType == "Position Direct") { - m_controlMode = wbi::CTRL_MODE_DIRECT_POSITION; - } else if (controlType == "Velocity") { - m_controlMode = wbi::CTRL_MODE_VEL; - } else if (controlType == "Torque") { - m_controlMode = wbi::CTRL_MODE_TORQUE; - } else if (controlType == "Open Loop") { - m_controlMode = wbi::CTRL_MODE_MOTOR_PWM; - } else { - if (error) error->message = "Control Mode not supported"; + // Set the control mode to all the controlledJoints + if (!icmd2->setControlModes(m_controlModes.data())) { + Log::getSingleton().error("Failed to set control mode."); return false; } + } + + // Get the signal + Signal references = blockInfo->getInputPortSignal(0); + unsigned signalWidth = blockInfo->getInputPortWidth(0); - //Read if full or sublist control - m_fullControl = blockInfo->getScalarParameterAtIndex(WBIBlock::numberOfParameters() + 2).booleanData(); + double* bufferReferences = references.getBuffer(); + if (!bufferReferences) { + Log::getSingleton().error("Failed to get the buffer containing references."); + return false; + } - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - if (!m_fullControl) { - //sublist - std::string controlledList; - if (!blockInfo->getStringParameterAtIndex(WBIBlock::numberOfParameters() + 3, controlledList)) { - if (error) error->message = "Could not read control type parameter"; + bool ok = false; + // TODO: here only the first element is checked + switch (m_controlModes.front()) { + case VOCAB_CM_UNKNOWN: + Log::getSingleton().error("Control mode has not been successfully set."); + return false; + break; + case VOCAB_CM_POSITION: { + // Get the interface + IPositionControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get IPositionControl interface."); return false; } - const yarp::os::Property * controlledListProperty = WBInterface::sharedInstance().currentConfiguration(); - - wbi::IDList idList; - bool result = WBInterface::wbdIDListFromConfigPropAndList(*controlledListProperty, - controlledList, idList); - wbi::IDList fullList = WBInterface::sharedInstance().interface()->getJointList(); - m_controlledJoints.clear(); - m_controlledJoints.reserve(idList.size()); - if (result) { - for (int i = 0; i < idList.size(); i++) { - int index; - if (fullList.idToIndex(idList.at(i), index)) - m_controlledJoints.push_back(index); - else - std::cerr << "Joint " << idList.at(i).toString() << " not found\n"; - } - } - dofs = idList.size(); + // Convert from rad to deg + auto referencesDeg = rad2deg(bufferReferences, signalWidth); + // Set the references + ok = interface->positionMove(referencesDeg.data()); + break; } - m_references = new double[dofs]; - - m_resetControlMode = true; - return m_references; - } - - bool SetReferences::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); - if (interface) { - if (m_fullControl) { - interface->setControlMode(wbi::CTRL_MODE_POS); - } else { - for (int i = 0; i < m_controlledJoints.size(); i++) { - interface->setControlMode(wbi::CTRL_MODE_POS, 0, m_controlledJoints[i]); - } + case VOCAB_CM_POSITION_DIRECT: { + // Get the interface + IPositionDirect* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get IPositionDirect interface."); + return false; } + // Convert from rad to deg + auto referencesDeg = rad2deg(bufferReferences, signalWidth); + // Set the references + ok = interface->setPositions(referencesDeg.data()); + break; } - if (m_references) { - delete [] m_references; - m_references = 0; + case VOCAB_CM_VELOCITY: { + // Get the interface + IVelocityControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get IVelocityControl interface."); + return false; + } + // Convert from rad to deg + auto referencesDeg = rad2deg(bufferReferences, signalWidth); + // Set the references + ok = interface->velocityMove(referencesDeg.data()); + break; } - return WBIBlock::terminate(blockInfo, error); - } - - bool SetReferences::initializeInitialConditions(BlockInformation */*blockInfo*/, wbt::Error */*error*/) - { - //Simply reset the variable m_resetControlMode - //It will be read at the first cycle of output - m_resetControlMode = true; - return true; - } - - bool SetReferences::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - //get input - wbi::wholeBodyInterface * const interface = WBInterface::sharedInstance().interface(); - if (!interface) return false; - - Signal references = blockInfo->getInputPortSignal(0); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - m_references[i] = references.get(i).doubleData(); + case VOCAB_CM_TORQUE: { + // Get the interface + ITorqueControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get ITorqueControl interface."); + return false; + } + // Set the references + ok = interface->setRefTorques(bufferReferences); + break; } - - if (m_resetControlMode) { - m_resetControlMode = false; - - //now switch control mode - if (m_fullControl) { - interface->setControlMode(m_controlMode, m_references); - } else { - for (int i = 0; i < m_controlledJoints.size(); i++) { - interface->setControlMode(m_controlMode, &m_references[i], m_controlledJoints[i]); - } + case VOCAB_CM_PWM: { + // Get the interface + IPWMControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get IPWMControl interface."); + return false; } - return true; + // Set the references + ok = interface->setRefDutyCycles(bufferReferences); + break; } - - if (m_fullControl) { - interface->setControlReference(m_references); - } else { - for (int i = 0; i < m_controlledJoints.size(); i++) { - interface->setControlReference(&m_references[i], m_controlledJoints[i]); + case VOCAB_CM_CURRENT: { + // Get the interface + ICurrentControl* interface = nullptr; + if (!getRobotInterface()->getInterface(interface) || !interface) { + Log::getSingleton().error("Failed to get ICurrentControl interface."); + return false; } + // Set the references + ok = interface->setRefCurrents(bufferReferences); + break; } - return true; } + + if (!ok) { + Log::getSingleton().error("Failed to set references."); + return false; + } + + return true; } diff --git a/toolbox/src/SimulatorSynchronizer.cpp b/toolbox/src/SimulatorSynchronizer.cpp index 6ed88c3e4..c46c528e1 100644 --- a/toolbox/src/SimulatorSynchronizer.cpp +++ b/toolbox/src/SimulatorSynchronizer.cpp @@ -1,132 +1,142 @@ #include "SimulatorSynchronizer.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" +#include "thrift/ClockServer.h" #include #include -#include #include -#define PARAM_IDX_1 1 // Period -#define PARAM_IDX_2 2 // Gazebo clock port -#define PARAM_IDX_3 3 // RPC client port name +using namespace wbt; +struct SimulatorSynchronizer::RPCData +{ + struct { + std::string clientPortName; + std::string serverPortName; -namespace wbt { + unsigned numberOfSteps; + } configuration; - struct SimulatorSynchronizer::RPCData - { - struct { - std::string clientPortName; - std::string serverPortName; + yarp::os::Port clientPort; + gazebo::ClockServer clockServer; +}; - unsigned numberOfSteps; - } configuration; +const std::string SimulatorSynchronizer::ClassName = "SimulatorSynchronizer"; - yarp::os::Port clientPort; - gazebo::ClockServer clockServer; - }; - - std::string SimulatorSynchronizer::ClassName = "SimulatorSynchronizer"; +const unsigned SimulatorSynchronizer::PARAM_PERIOD = 1; +const unsigned SimulatorSynchronizer::PARAM_GZCLK_PORT = 2; +const unsigned SimulatorSynchronizer::PARAM_RPC_PORT = 3; - SimulatorSynchronizer::SimulatorSynchronizer() - : m_period(0.01) - , m_firstRun(true) - , m_rpcData(0) {} +SimulatorSynchronizer::SimulatorSynchronizer() +: m_period(0.01) +, m_firstRun(true) +{} - SimulatorSynchronizer::~SimulatorSynchronizer() {} - - unsigned SimulatorSynchronizer::numberOfParameters() { return 3; } +unsigned SimulatorSynchronizer::numberOfParameters() { return 3; } - std::vector SimulatorSynchronizer::additionalBlockOptions() - { - return std::vector(1, wbt::BlockOptionPrioritizeOrder); - } - - bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - // Specify I/O - // INPUTS - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; - return false; - } - - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to set output port number"; - return false; - } +std::vector SimulatorSynchronizer::additionalBlockOptions() +{ + return std::vector(1, wbt::BlockOptionPrioritizeOrder); +} - return true; +bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; } - bool SimulatorSynchronizer::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - m_period = blockInfo->getScalarParameterAtIndex(PARAM_IDX_1).doubleData(); - std::string serverPortName; - std::string clientPortName; + // OUTPUT + // ====== + // + // No outputs + // - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_2, serverPortName) || !blockInfo->getStringParameterAtIndex(PARAM_IDX_3, clientPortName)) { - if (error) error->message = "Error reading RPC parameters"; - return false; - } + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } - yarp::os::Network::init(); - if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork()) { - if (error) error->message = "Error initializing Yarp network"; - return false; - } + return true; +} - m_rpcData = new struct RPCData(); - if (!m_rpcData) { - if (error) error->message = "Error creating RPC data structure"; - return false; - } +bool SimulatorSynchronizer::initialize(const BlockInformation* blockInfo) +{ + std::string serverPortName; + std::string clientPortName; - m_rpcData->configuration.clientPortName = clientPortName; - m_rpcData->configuration.serverPortName = serverPortName; + bool ok = true; + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_GZCLK_PORT, serverPortName); + ok = ok && blockInfo->getStringParameterAtIndex(PARAM_RPC_PORT, clientPortName); - m_firstRun = true; + if (!ok) { + Log::getSingleton().error("Error reading RPC parameters."); + return false; + } - return true; + yarp::os::Network::init(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork()) { + Log::getSingleton().error("Error initializing Yarp network."); + return false; } - - bool SimulatorSynchronizer::terminate(BlockInformation */*S*/, wbt::Error *error) - { - if (m_rpcData) { - if (m_rpcData->clientPort.isOpen()) { - m_rpcData->clockServer.continueSimulation(); - if (!yarp::os::Network::disconnect(m_rpcData->configuration.clientPortName, m_rpcData->configuration.serverPortName)) { - if (error) error->message = "Error disconnecting from simulator clock server"; - } - m_rpcData->clientPort.close(); - } - delete m_rpcData; - m_rpcData = 0; - } - yarp::os::Network::fini(); - return true; + + m_rpcData = std::unique_ptr(new struct RPCData()); + if (!m_rpcData) { + Log::getSingleton().error("Error creating RPC data structure."); + return false; } - bool SimulatorSynchronizer::output(BlockInformation */*S*/, wbt::Error *error) - { - if (m_firstRun) { - m_firstRun = false; + m_rpcData->configuration.clientPortName = clientPortName; + m_rpcData->configuration.serverPortName = serverPortName; + + m_firstRun = true; - if (!m_rpcData->clientPort.open(m_rpcData->configuration.clientPortName) - || !yarp::os::Network::connect(m_rpcData->configuration.clientPortName, m_rpcData->configuration.serverPortName)) { - if (error) error->message = "Error connecting to simulator clock server"; - return false; + return true; +} + +bool SimulatorSynchronizer::terminate(const BlockInformation* /*S*/) +{ + if (m_rpcData) { + if (m_rpcData->clientPort.isOpen()) { + m_rpcData->clockServer.continueSimulation(); + if (!yarp::os::Network::disconnect(m_rpcData->configuration.clientPortName, + m_rpcData->configuration.serverPortName)) { + Log::getSingleton().error("Error disconnecting from simulator clock server."); } + m_rpcData->clientPort.close(); + } + } + yarp::os::Network::fini(); + return true; +} - m_rpcData->clockServer.yarp().attachAsClient(m_rpcData->clientPort); +bool SimulatorSynchronizer::output(const BlockInformation* /*S*/) +{ + if (m_firstRun) { + m_firstRun = false; - double stepSize = m_rpcData->clockServer.getStepSize(); - m_rpcData->configuration.numberOfSteps = static_cast(m_period / stepSize); - m_rpcData->clockServer.pauseSimulation(); + if (!m_rpcData->clientPort.open(m_rpcData->configuration.clientPortName) || + !yarp::os::Network::connect(m_rpcData->configuration.clientPortName, + m_rpcData->configuration.serverPortName)) { + Log::getSingleton().error("Error connecting to simulator clock server."); + return false; } - m_rpcData->clockServer.stepSimulationAndWait(m_rpcData->configuration.numberOfSteps); - return true; + m_rpcData->clockServer.yarp().attachAsClient(m_rpcData->clientPort); + + double stepSize = m_rpcData->clockServer.getStepSize(); + m_rpcData->configuration.numberOfSteps = static_cast(m_period / stepSize); + m_rpcData->clockServer.pauseSimulation(); } + m_rpcData->clockServer.stepSimulationAndWait(m_rpcData->configuration.numberOfSteps); + + return true; } diff --git a/toolbox/src/YARPWBIConverter.cpp b/toolbox/src/YARPWBIConverter.cpp deleted file mode 100644 index 0cca8ff77..000000000 --- a/toolbox/src/YARPWBIConverter.cpp +++ /dev/null @@ -1,268 +0,0 @@ -#include "YARPWBIConverter.h" - -#include "BlockInformation.h" -#include "Signal.h" -#include "Error.h" -#include "WBInterface.h" -#include -#include -#include -#include - - -namespace wbt { - namespace hidden { - struct Joint; - - struct Part { - std::string partName; - std::vector joints; - }; - - struct Joint { - unsigned yarpIndex; - unsigned dofIndex; - std::string name; - }; - - /** - * Parse the configuration file to extract parts, joints and indices - * - * @param filepath full path and name to the configuration file - * @return the YARP parts representation - */ - static bool parseConfigurationFileForPartsConfiguration(const std::string& filepath, const wbi::IDList& wbiList, std::vector &parts); - - } -} - -namespace wbt { - - struct YARPWBIConverter::YARPWBIConverterPimpl { - std::vector parts; - bool yarpToWBI; - }; - - std::string YARPWBIConverter::ClassName = "YARPWBIConverter"; - - YARPWBIConverter::YARPWBIConverter() {} - - unsigned YARPWBIConverter::numberOfParameters() { return wbt::WBIModelBlock::numberOfParameters() + 1; } - bool YARPWBIConverter::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - //This is a check that list, dofs and files are ok - if (!WBIBlock::configureSizeAndPorts(blockInfo, error)) { - return false; - } - - //This specify the output port size - unsigned dofs = WBInterface::sharedInstance().numberOfDoFs(); - - using namespace wbt::hidden; - //Parse the yarpwbi file - //Need to retrieve both parameters, i.e. wbiFile and list - //The following is to avoid to configure the WBInterface during compilation - //(why I don't know :D ) - std::vector parts; - - //Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 - yarp::os::Network network; - yarp::os::ResourceFinder resourceFinder = yarp::os::ResourceFinder::getResourceFinderSingleton(); - - std::string configFilePath = resourceFinder.findFile(m_wbiConfigurationFileName); - - yarp::os::Property configurations; - //loading defaults from configuration file - if (!configurations.fromConfigFile(configFilePath)) { - if (error) error->message = "Failed to parse WBI configuration file"; - return false; - } - - wbi::IDList jointList; - //parse the file to get the joint list - if (!WBInterface::wbdIDListFromConfigPropAndList(configurations, m_wbiListName, jointList)) { - if (error) error->message = "Failed to parse Joint list"; - return false; - } - - if (!parseConfigurationFileForPartsConfiguration(configFilePath, jointList, parts)) { - return false; - } - - blockInfo->getScalarParameterAtIndex(wbt::WBIModelBlock::numberOfParameters() + 1).int8Data(); - - bool yarpToWBI = blockInfo->getScalarParameterAtIndex(wbt::WBIModelBlock::numberOfParameters() + 1).int8Data() == 1; - - // Specify I/O - bool success = true; - //Input - if (yarpToWBI) { - if (!blockInfo->setNumberOfInputPorts(parts.size())) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - for (unsigned i = 0 ; i < parts.size(); ++i) { - success = success && blockInfo->setInputPortVectorSize(i, parts[i].joints.size()); - blockInfo->setInputPortType(i, PortDataTypeDouble); - } - - } else { - if (!blockInfo->setNumberOfInputPorts(1)) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - success = success && blockInfo->setInputPortVectorSize(0, dofs); - blockInfo->setInputPortType(0, PortDataTypeDouble); - } - - if (!success) { - if (error) error->message = "Failed to configure input ports"; - return false; - } - - if (yarpToWBI) { - // Output port: - // - one port of size dofs - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to configure the number of output ports"; - return false; - } - - success = blockInfo->setOutputPortVectorSize(0, dofs); - blockInfo->setOutputPortType(0, PortDataTypeDouble); - } else { - if (!blockInfo->setNumberOfOuputPorts(parts.size())) { - if (error) error->message = "Failed to configure the number of input ports"; - return false; - } - - for (unsigned i = 0 ; i < parts.size(); ++i) { - success = success && blockInfo->setOutputPortVectorSize(i, parts[i].joints.size()); - blockInfo->setOutputPortType(i, PortDataTypeDouble); - } - } - - if (!success) { - if (error) error->message = "Failed to configure output ports"; - return false; - } - - return success; - } - - bool YARPWBIConverter::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace wbt::hidden; - if (!WBIModelBlock::initialize(blockInfo, error)) return false; - - m_pimpl = new YARPWBIConverterPimpl(); - if (!m_pimpl) return false; - - m_pimpl->yarpToWBI = blockInfo->getScalarParameterAtIndex(wbt::WBIModelBlock::numberOfParameters() + 1).int8Data() == 1; - return parseConfigurationFileForPartsConfiguration(WBInterface::sharedInstance().wbiFilePath(), *WBInterface::sharedInstance().wbiList(), m_pimpl->parts); - } - - bool YARPWBIConverter::terminate(BlockInformation *blockInfo, wbt::Error *error) - { - delete m_pimpl; - m_pimpl = 0; - - return WBIModelBlock::terminate(blockInfo, error); - } - - bool YARPWBIConverter::output(BlockInformation *blockInfo, wbt::Error *error) - { - if (!m_pimpl) return false; - using namespace wbt::hidden; - - if (m_pimpl->yarpToWBI) { - //From YARP to WBI: - //Multiple inputs, single output - - Signal output = blockInfo->getOutputPortSignal(0); - - for (unsigned partIdx = 0; partIdx < m_pimpl->parts.size(); ++partIdx) { - Signal partPort = blockInfo->getInputPortSignal(partIdx); - - Part part = m_pimpl->parts[partIdx]; - for (unsigned jointIdx = 0; jointIdx < part.joints.size(); ++jointIdx) { - Joint joint = part.joints[jointIdx]; - output.set(joint.dofIndex, partPort.get(joint.yarpIndex).doubleData()); - } - } - - } else { - //From WBI to YARP - //Single Input port, multiple outputs - Signal inputPort = blockInfo->getInputPortSignal(0); - - for (unsigned partIdx = 0; partIdx < m_pimpl->parts.size(); ++partIdx) { - Signal output = blockInfo->getOutputPortSignal(partIdx); - - Part part = m_pimpl->parts[partIdx]; - for (unsigned jointIdx = 0; jointIdx < part.joints.size(); ++jointIdx) { - Joint joint = part.joints[jointIdx]; - output.set(joint.yarpIndex, inputPort.get(joint.dofIndex).doubleData()); - } - - } - } - - return true; - } - - namespace hidden { - - bool parseConfigurationFileForPartsConfiguration(const std::string& filepath, const wbi::IDList& wbiList, std::vector &parts) - { - parts.clear(); - yarp::os::Property configurationFile; - configurationFile.fromConfigFile(filepath); - //retrieve joints/yarp mapping - yarp::os::Bottle &mapping = configurationFile.findGroup("WBI_YARP_JOINTS"); - if (mapping.isNull() || mapping.size() <= 0) { - return false; - } - - //for every joint in the list - for (unsigned i = 0; i < wbiList.size(); ++i) { - const wbi::ID &id = wbiList.at(i); - //look in the configuration file for that key - yarp::os::Value &jointList = mapping.find(id.toString()); - - if (!jointList.isNull() && jointList.isList() && jointList.asList()->size() == 2) { - yarp::os::Bottle *list = jointList.asList(); - Joint joint; - std::string partName = list->get(0).asString(); - joint.yarpIndex = list->get(1).asInt(); - joint.dofIndex = i; - joint.name = id.toString(); - - std::vector::iterator found = parts.end(); - //look for the part in the vector.. a map would be nicer - for (std::vector::iterator part = parts.begin(); - part != parts.end(); ++part) { - if (part->partName == partName) { - found = part; - break; - } - } - - if (found != parts.end()) { - found->joints.push_back(joint); - } else { - Part newPart; - newPart.partName = partName; - newPart.joints.push_back(joint); - parts.push_back(newPart); - } - } - } - return true; - } - } -} - - diff --git a/toolbox/src/YarpClock.cpp b/toolbox/src/YarpClock.cpp index f20ba9b5b..67592823e 100644 --- a/toolbox/src/YarpClock.cpp +++ b/toolbox/src/YarpClock.cpp @@ -1,60 +1,69 @@ #include "YarpClock.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" #include #include -namespace wbt { +using namespace wbt; - std::string YarpClock::ClassName = "YarpClock"; +const std::string YarpClock::ClassName = "YarpClock"; - unsigned YarpClock::numberOfParameters() { return 0; } +unsigned YarpClock::numberOfParameters() { return 0; } - bool YarpClock::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - // Specify I/O - // INPUTS - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; - return false; - } +bool YarpClock::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // - // OUTPUTS - if (!blockInfo->setNumberOfOuputPorts(1)) { - if (error) error->message = "Failed to set output port number"; - return false; - } + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } - blockInfo->setOutputPortVectorSize(0, 1); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + // OUTPUT + // ====== + // + // 1) The yarp time. In short, it streams yarp::os::Time::now(). + // - return true; + if (!blockInfo->setNumberOfOutputPorts(1)) { + Log::getSingleton().error("Failed to set output port number."); + return false; } - bool YarpClock::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - yarp::os::Network::init(); + blockInfo->setOutputPortVectorSize(0, 1); + blockInfo->setOutputPortType(0, PortDataTypeDouble); - if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)){ - if (error) error->message = "YARP server wasn't found active!! \n"; - return false; - } + return true; +} - return true; - } - bool YarpClock::terminate(BlockInformation */*S*/, wbt::Error */*error*/) - { - yarp::os::Network::fini(); - return true; - } +bool YarpClock::initialize(const BlockInformation* blockInfo) +{ + yarp::os::Network::init(); - bool YarpClock::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - Signal signal = blockInfo->getOutputPortSignal(0); - signal.set(0, yarp::os::Time::now()); - return true; + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; } + + return true; +} + +bool YarpClock::terminate(const BlockInformation* /*S*/) +{ + yarp::os::Network::fini(); + return true; +} + +bool YarpClock::output(const BlockInformation* blockInfo) +{ + Signal signal = blockInfo->getOutputPortSignal(0); + signal.set(0, yarp::os::Time::now()); + return true; } diff --git a/toolbox/src/YarpRead.cpp b/toolbox/src/YarpRead.cpp index 9fd15c3ca..a25dc31db 100644 --- a/toolbox/src/YarpRead.cpp +++ b/toolbox/src/YarpRead.cpp @@ -1,6 +1,6 @@ #include "YarpRead.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" @@ -10,165 +10,218 @@ #include #include +#include + +using namespace wbt; + +const std::string YarpRead::ClassName = "YarpRead"; + +const unsigned YarpRead::PARAM_IDX_PORTNAME = 1; // port name +const unsigned YarpRead::PARAM_IDX_PORTSIZE = 2; // Size of the port you're reading +const unsigned YarpRead::PARAM_IDX_WAITDATA = 3; // boolean for blocking reading +const unsigned YarpRead::PARAM_IDX_READ_TS = 4; // boolean to stream timestamp +const unsigned YarpRead::PARAM_IDX_AUTOCONNECT = 5; // Autoconnect boolean +const unsigned YarpRead::PARAM_IDX_ERR_NO_PORT = 6; // Error on missing port if autoconnect is on boolean + +YarpRead::YarpRead() +: m_autoconnect(false) +, m_blocking(false) +, m_shouldReadTimestamp(false) +, m_errorOnMissingPort(true) +{} + +unsigned YarpRead::numberOfParameters() { return 6; } + +bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUTS + // ====== + // + // No inputs + // + + if (!blockInfo->setNumberOfInputPorts(0)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } -#define PARAM_IDX_1 1 // port name -#define PARAM_IDX_2 2 // Size of the port you're reading -#define PARAM_IDX_3 3 // boolean for blocking reading -#define PARAM_IDX_4 4 // boolean to stream timestamp -#define PARAM_IDX_5 5 // Autoconnect boolean -#define PARAM_IDX_6 6 // Error on missing port if autoconnect is on boolean - -namespace wbt { - - std::string YarpRead::ClassName = "YarpRead"; - - YarpRead::YarpRead() - : m_autoconnect(false) - , m_blocking(false) - , m_shouldReadTimestamp(false) - , m_errorOnMissingPort(true) - , m_port(0) {} - - unsigned YarpRead::numberOfParameters() { return 6; } - - bool YarpRead::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - // Specify I/O - // INPUTS - if (!blockInfo->setNumberOfInputPorts(0)) { - if (error) error->message = "Failed to set input port number to 0"; - return false; - } - - // OUTPUTS - bool shouldReadTimestamp = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); - int signalSize = blockInfo->getScalarParameterAtIndex(PARAM_IDX_2).int32Data(); - bool autoconnect = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); - - if (signalSize < 0) { - if (error) error->message = "Signal size must be non negative"; - return false; - } + // OUTPUTS + // ======= + // + // 1) Vector with the data read from the port (1 x signalSize) + // 2) Optional: Timestamp read from the port + // 3) Optional: If autoconnect is disabled, this output becomes 1 when data starts arriving + // (and hence it means that the user connected manually the port) + // - int numberOfOutputPorts = 1; - if (shouldReadTimestamp) numberOfOutputPorts++; //timestamp is the second port - if (!autoconnect) numberOfOutputPorts++; //!autoconnect => additional port with 1/0 depending on the connection status + bool shouldReadTimestamp; + bool autoconnect; + double signalSize; - if (!blockInfo->setNumberOfOuputPorts(numberOfOutputPorts)) { - if (error) error->message = "Failed to set output port number"; - return false; - } + bool ok = true; - blockInfo->setOutputPortVectorSize(0, signalSize); - blockInfo->setOutputPortType(0, PortDataTypeDouble); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, shouldReadTimestamp); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, autoconnect); + ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_PORTSIZE, signalSize); - int portIndex = 1; - if (shouldReadTimestamp) { - blockInfo->setOutputPortVectorSize(portIndex, 2); - blockInfo->setOutputPortType(portIndex, PortDataTypeDouble); - portIndex++; - } - if (!autoconnect) { - blockInfo->setOutputPortVectorSize(portIndex, 1); - blockInfo->setOutputPortType(portIndex, PortDataTypeInt8); //use double anyway. Simplifies simulink stuff - portIndex++; - } - return true; + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; } - bool YarpRead::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - using namespace yarp::sig; + if (signalSize < 0) { + Log::getSingleton().error("Signal size must be non negative."); + return false; + } - Network::init(); + int numberOfOutputPorts = 1; + numberOfOutputPorts += static_cast(shouldReadTimestamp); //timestamp is the second port + numberOfOutputPorts += static_cast(!autoconnect); //!autoconnect => additional port with 1/0 depending on the connection status - if (!Network::initialized() || !Network::checkNetwork(5.0)){ - if (error) error->message = "YARP server wasn't found active!! \n"; - return false; - } + if (!blockInfo->setNumberOfOutputPorts(numberOfOutputPorts)) { + Log::getSingleton().error("Failed to set output port number."); + return false; + } - m_shouldReadTimestamp = blockInfo->getScalarParameterAtIndex(PARAM_IDX_4).booleanData(); - m_autoconnect = blockInfo->getScalarParameterAtIndex(PARAM_IDX_5).booleanData(); - m_blocking = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); - m_errorOnMissingPort = blockInfo->getScalarParameterAtIndex(PARAM_IDX_6).booleanData(); + blockInfo->setOutputPortVectorSize(0, static_cast(signalSize)); + blockInfo->setOutputPortType(0, PortDataTypeDouble); - std::string portName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_1, portName)) { - if (error) error->message = "Cannot retrieve string from port name parameter"; - return false; - } + int portIndex = 1; + if (shouldReadTimestamp) { + blockInfo->setOutputPortVectorSize(portIndex, 2); + blockInfo->setOutputPortType(portIndex, PortDataTypeDouble); + portIndex++; + } + if (!autoconnect) { + blockInfo->setOutputPortVectorSize(portIndex, 1); + blockInfo->setOutputPortType(portIndex, PortDataTypeInt8); //use double anyway. Simplifies simulink stuff + portIndex++; + } + return true; +} - std::string sourcePortName; - std::string destinationPortName; +bool YarpRead::initialize(const BlockInformation* blockInfo) +{ + using namespace yarp::os; + using namespace yarp::sig; - if (m_autoconnect) { - sourcePortName = portName; - destinationPortName = "..."; - } else { - destinationPortName = portName; - } + Network::init(); + + if (!Network::initialized() || !Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } - m_port = new BufferedPort(); + bool ok = true; - if (!m_port || !m_port->open(destinationPortName)) { - if (error) error->message = "Error while opening yarp port"; - return false; - } + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_READ_TS, m_shouldReadTimestamp); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_WAITDATA, m_blocking); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); - if (m_autoconnect) { - if (!Network::connect(sourcePortName, m_port->getName())) { - std::cerr << "Failed to connect " << sourcePortName << " to " << m_port->getName() << "\n"; - if (m_errorOnMissingPort) { - if (error) error->message = "ERROR connecting ports"; - return false; - } + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } + + std::string portName; + if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portName)) { + Log::getSingleton().error("Cannot retrieve string from port name parameter."); + return false; + } + + std::string sourcePortName; + std::string destinationPortName; + + // Autoconnect: the block opens a temporary input port ..., and it connects to an existing + // port portName (which is streaming data). + if (m_autoconnect) { + sourcePortName = portName; + destinationPortName = "..."; + } + // Manual connection: the block opens an input port portName, and waits a manual connection to an + // output port. + else { + destinationPortName = portName; + } + + m_port = std::unique_ptr>(new BufferedPort()); + + if (!m_port || !m_port->open(destinationPortName)) { + Log::getSingleton().error("Error while opening yarp port."); + return false; + } + + if (m_autoconnect) { + if (!Network::connect(sourcePortName, m_port->getName())) { + Log::getSingleton().warning("Failed to connect " + + sourcePortName + + " to " + + m_port->getName()); + if (m_errorOnMissingPort) { + Log::getSingleton().error("Failed connecting ports."); + return false; } } - return true; - } - bool YarpRead::terminate(BlockInformation */*S*/, wbt::Error */*error*/) - { - if (m_port) { - m_port->close(); - delete m_port; - m_port = 0; - } - yarp::os::Network::fini(); - return true; - } - - bool YarpRead::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - int timeStampPortIndex = 1; - int connectionStatusPortIndex = 1; - - yarp::sig::Vector *v = m_port->read(m_blocking); // Read from the port. Waits until data arrives. - if (v) - { - if (m_shouldReadTimestamp) { - connectionStatusPortIndex++; - yarp::os::Stamp timestamp; - m_port->getEnvelope(timestamp); - - Signal pY1 = blockInfo->getOutputPortSignal(timeStampPortIndex); - pY1.set(0, timestamp.getCount()); - pY1.set(1, timestamp.getTime()); + } + return true; +} - } - Signal signal = blockInfo->getOutputPortSignal(0); - signal.setBuffer(v->data(), std::min(blockInfo->getOutputPortWidth(0), (unsigned)v->size())); +bool YarpRead::terminate(const BlockInformation* /*blockInfo*/) +{ + if (m_port) { + m_port->close(); + } + yarp::os::Network::fini(); + return true; +} - if (!m_autoconnect) { - Signal statusPort = blockInfo->getOutputPortSignal(connectionStatusPortIndex); - statusPort.set(0, 1); //somebody wrote in the port => the port is connected +bool YarpRead::output(const BlockInformation* blockInfo) +{ + int timeStampPortIndex = 0; + int connectionStatusPortIndex = 0; - //TODO implement a sort of "timeout" paramter - //At the current state this is equal to timeout = inf - //Otherwise we can check the timeout and if nobody sent data in the last X secs - //we set the port to zero again + if (m_shouldReadTimestamp) { + timeStampPortIndex = 1; + } + if (!m_autoconnect) { + connectionStatusPortIndex = timeStampPortIndex + 1; + } + + yarp::sig::Vector* v = m_port->read(m_blocking); // Read from the port. Waits until data arrives. + + if (v) { + if (m_shouldReadTimestamp) { + connectionStatusPortIndex++; + yarp::os::Stamp timestamp; + if (!m_port->getEnvelope(timestamp)) { + Log::getSingleton().error("Failed to read port envelope (timestamp)."); + Log::getSingleton().errorAppend("Be sure that the input port actually writes this data."); + return false; } + + Signal pY1 = blockInfo->getOutputPortSignal(timeStampPortIndex); + pY1.set(0, timestamp.getCount()); + pY1.set(1, timestamp.getTime()); + } + + Signal signal = blockInfo->getOutputPortSignal(0); + + // Crop the buffer if it exceeds the OutputPortWidth. + signal.setBuffer(v->data(), + std::min(blockInfo->getOutputPortWidth(0), + static_cast(v->size()))); + + if (!m_autoconnect) { + Signal statusPort = blockInfo->getOutputPortSignal(connectionStatusPortIndex); + statusPort.set(0, 1); //somebody wrote in the port => the port is connected + + //TODO implement a sort of "timeout" paramter + //At the current state this is equal to timeout = inf + //Otherwise we can check the timeout and if nobody sent data in the last X secs + //we set the port to zero again } - return true; } + + return true; } diff --git a/toolbox/src/YarpWrite.cpp b/toolbox/src/YarpWrite.cpp index a6f920c04..bb445a405 100644 --- a/toolbox/src/YarpWrite.cpp +++ b/toolbox/src/YarpWrite.cpp @@ -1,6 +1,6 @@ #include "YarpWrite.h" -#include "Error.h" +#include "Log.h" #include "BlockInformation.h" #include "Signal.h" @@ -9,116 +9,144 @@ #include #include -#define PARAM_IDX_1 1 // port name -#define PARAM_IDX_2 2 // Autoconnect boolean -#define PARAM_IDX_3 3 // Error on missing port if autoconnect is on boolean - -namespace wbt { - - std::string YarpWrite::ClassName = "YarpWrite"; - - YarpWrite::YarpWrite() - : m_autoconnect(false) - , m_errorOnMissingPort(true) - , m_destinationPortName("") - , m_port(0) {} - - unsigned YarpWrite::numberOfParameters() { return 3; } - - bool YarpWrite::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) - { - if (!blockInfo->setNumberOfInputPorts(1)) { - if (error) error->message = "Failed to set input port number to 0"; - return false; - } - blockInfo->setInputPortVectorSize(0, -1); - blockInfo->setInputPortType(0, PortDataTypeDouble); +using namespace wbt; - if (!blockInfo->setNumberOfOuputPorts(0)) { - if (error) error->message = "Failed to set output port number"; - return false; - } - return true; +const std::string YarpWrite::ClassName = "YarpWrite"; + +const unsigned YarpWrite::PARAM_IDX_PORTNAME = 1; // Port name +const unsigned YarpWrite::PARAM_IDX_AUTOCONNECT = 2; // Autoconnect boolean +const unsigned YarpWrite::PARAM_IDX_ERR_NO_PORT = 3; // Error on missing port if autoconnect is true + +YarpWrite::YarpWrite() +: m_autoconnect(false) +, m_errorOnMissingPort(true) +, m_destinationPortName("") +{} + +unsigned YarpWrite::numberOfParameters() { return 3; } + +bool YarpWrite::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // INPUT + // ===== + // + // 1) The signal to stream to the specified yarp port + // + + if (!blockInfo->setNumberOfInputPorts(1)) { + Log::getSingleton().error("Failed to set input port number to 0."); + return false; + } + blockInfo->setInputPortVectorSize(0, -1); + blockInfo->setInputPortType(0, PortDataTypeDouble); + + // OUTPUT + // ====== + // + // No outputs + // + + if (!blockInfo->setNumberOfOutputPorts(0)) { + Log::getSingleton().error("Failed to set output port number."); + return false; } - bool YarpWrite::initialize(BlockInformation *blockInfo, wbt::Error *error) - { - using namespace yarp::os; - using namespace yarp::sig; + return true; +} - Network::init(); +bool YarpWrite::initialize(const BlockInformation* blockInfo) +{ + using namespace yarp::os; + using namespace yarp::sig; - if (!Network::initialized() || !Network::checkNetwork(5.0)){ - if (error) error->message = "YARP server wasn't found active!! \n"; - return false; - } + Network::init(); - m_autoconnect = blockInfo->getScalarParameterAtIndex(PARAM_IDX_2).booleanData(); - m_errorOnMissingPort = blockInfo->getScalarParameterAtIndex(PARAM_IDX_3).booleanData(); + if (!Network::initialized() || !Network::checkNetwork(5.0)){ + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } - std::string portParameter; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_1, portParameter)) { - if (error) error->message = "Error reading port name parameter"; - return false; - } + bool ok = true; - std::string sourcePortName; + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_AUTOCONNECT, m_autoconnect); + ok = ok && blockInfo->getBooleanParameterAtIndex(PARAM_IDX_ERR_NO_PORT, m_errorOnMissingPort); - if (m_autoconnect) { - sourcePortName = "..."; - m_destinationPortName = portParameter; - } else { - sourcePortName = portParameter; - } + if (!ok) { + Log::getSingleton().error("Failed to read input parameters."); + return false; + } - m_port = new BufferedPort(); + std::string portParameter; + if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_PORTNAME, portParameter)) { + Log::getSingleton().error("Error reading port name parameter."); + return false; + } - if (!m_port || !m_port->open(sourcePortName)) { - if (error) error->message = "Error while opening yarp port"; - return false; - } + std::string sourcePortName; - if (m_autoconnect) { - if (!Network::connect(m_port->getName(), m_destinationPortName)) { - if (m_errorOnMissingPort) { - if (error) error->message ="Failed to connect " + m_port->getName() + " to " + m_destinationPortName; - return false; - } - } - } + // Autoconnect: the block opens a temporary output port ..., and it connects to an existing + // port portName (which will receive data). + if (m_autoconnect) { + sourcePortName = "..."; + m_destinationPortName = portParameter; + } + // Manual connection: the block opens an output port portName, and waits a manual connection to an + // input port. + else { + sourcePortName = portParameter; + } + + m_port = std::unique_ptr>(new BufferedPort()); - //prepare the first object allocation - yarp::sig::Vector &outputVector = m_port->prepare(); - outputVector.resize(blockInfo->getInputPortWidth(0)); - return true; + if (!m_port || !m_port->open(sourcePortName)) { + Log::getSingleton().error("Error while opening yarp port."); + return false; } - bool YarpWrite::terminate(BlockInformation */*S*/, wbt::Error */*error*/) - { - if (m_port) { - if (m_autoconnect) - yarp::os::Network::disconnect(m_port->getName(), m_destinationPortName); - m_port->close(); - delete m_port; - m_port = 0; + if (m_autoconnect) { + if (!Network::connect(m_port->getName(), m_destinationPortName)) { + Log::getSingleton().warning("Failed to connect " + + m_port->getName() + + " to " + + m_destinationPortName); + if (m_errorOnMissingPort) { + Log::getSingleton().error("Failed connecting ports."); + return false; + } } - yarp::os::Network::fini(); - return true; } - - bool YarpWrite::output(BlockInformation *blockInfo, wbt::Error */*error*/) - { - if (!m_port) return false; - yarp::sig::Vector &outputVector = m_port->prepare(); - outputVector.resize(blockInfo->getInputPortWidth(0)); //this should be a no-op - - Signal signal = blockInfo->getInputPortSignal(0); - for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { - outputVector[i] = signal.get(i).doubleData(); + + // Initialize the size of the internal buffer handled by m_port + yarp::sig::Vector& outputVector = m_port->prepare(); + outputVector.resize(blockInfo->getInputPortWidth(0)); + return true; +} + +bool YarpWrite::terminate(const BlockInformation* /*S*/) +{ + if (m_port) { + if (m_autoconnect) { + yarp::os::Network::disconnect(m_port->getName(), m_destinationPortName); } + m_port->close(); + } + yarp::os::Network::fini(); + return true; +} + +bool YarpWrite::output(const BlockInformation* blockInfo) +{ + if (!m_port) return false; + yarp::sig::Vector& outputVector = m_port->prepare(); + outputVector.resize(blockInfo->getInputPortWidth(0)); //this should be a no-op - m_port->write(); + Signal signal = blockInfo->getInputPortSignal(0); - return true; + for (unsigned i = 0; i < blockInfo->getInputPortWidth(0); ++i) { + outputVector[i] = signal.get(i); } + + m_port->write(); + + return true; } diff --git a/toolbox/src/base/Block.cpp b/toolbox/src/base/Block.cpp index 396e1084d..fd2e88731 100644 --- a/toolbox/src/base/Block.cpp +++ b/toolbox/src/base/Block.cpp @@ -1,15 +1,17 @@ #include "Block.h" #include "toolbox.h" -wbt::Block::~Block() {} -std::vector wbt::Block::additionalBlockOptions() { return std::vector(); } -void wbt::Block::parameterAtIndexIsTunable(unsigned /*index*/, bool &tunable) { tunable = false; } -bool wbt::Block::checkParameters(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } +using namespace wbt; -unsigned wbt::Block::numberOfDiscreteStates() { return 0; } -unsigned wbt::Block::numberOfContinuousStates() { return 0; } +Block::~Block() {} +std::vector Block::additionalBlockOptions() { return std::vector(); } +void Block::parameterAtIndexIsTunable(unsigned /*index*/, bool &tunable) { tunable = false; } +bool Block::checkParameters(const BlockInformation* /*blockInfo*/) { return true; } -bool wbt::Block::updateDiscreteState(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } -bool wbt::Block::stateDerivative(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } +unsigned Block::numberOfDiscreteStates() { return 0; } +unsigned Block::numberOfContinuousStates() { return 0; } -bool wbt::Block::initializeInitialConditions(wbt::BlockInformation */*blockInfo*/, wbt::Error */*error*/) { return true; } +bool Block::updateDiscreteState(const BlockInformation* /*blockInfo*/) { return true; } +bool Block::stateDerivative(const BlockInformation* /*blockInfo*/) { return true; } + +bool Block::initializeInitialConditions(const BlockInformation* /*blockInfo*/) { return true; } diff --git a/toolbox/src/base/BlockInformation.cpp b/toolbox/src/base/BlockInformation.cpp index c2fc14f0d..5e52eb899 100644 --- a/toolbox/src/base/BlockInformation.cpp +++ b/toolbox/src/base/BlockInformation.cpp @@ -2,5 +2,7 @@ const std::string wbt::BlockOptionPrioritizeOrder = "wbt.BlockOptionPrioritizeOrder"; -wbt::BlockInformation::~BlockInformation() {} -bool wbt::BlockInformation::optionFromKey(const std::string& key, wbt::Data &option) const { return false; } +bool wbt::BlockInformation::optionFromKey(const std::string& key, double& option) const +{ + return false; +} diff --git a/toolbox/src/base/Configuration.cpp b/toolbox/src/base/Configuration.cpp new file mode 100644 index 000000000..e4205ac9b --- /dev/null +++ b/toolbox/src/base/Configuration.cpp @@ -0,0 +1,145 @@ +#include "Configuration.h" +#include + +using namespace wbt; + +Configuration::Configuration(const std::string& confKey) +: m_confKey(confKey) +, m_dofs(0) +{} + +// SET METHODS +// =========== + +void Configuration::setParameters(const std::string& robotName, + const std::string& urdfFile, + const std::vector& controlledJoints, + const std::vector& controlBoardsNames, + const std::string& localName, + const std::array& gravityVector) +{ + setRobotName(robotName); + setUrdfFile(urdfFile); + setControlledJoints(controlledJoints); + setControlBoardsNames(controlBoardsNames); + setLocalName(localName); + setGravityVector(gravityVector); +} + +void Configuration::setRobotName(const std::string& robotName) +{ + m_robotName = robotName; +} + +void Configuration::setUrdfFile(const std::string& urdfFile) +{ + m_urdfFile = urdfFile; +} + +void Configuration::setControlledJoints(const std::vector& controlledJoints) +{ + m_controlledJoints = controlledJoints; + m_dofs = controlledJoints.size(); +} + +void Configuration::setControlBoardsNames(const std::vector& controlBoardsNames) +{ + m_controlBoardsNames = controlBoardsNames; +} + +void Configuration::setLocalName(const std::string& localName) +{ + m_localName = localName; + + // Add the leading "/" if missing + if (m_localName.compare(0, 1, "/")) { + m_localName = "/" + m_localName; + } +} + +const std::string Configuration::getUniqueId() const +{ + std::string uniqueId(m_confKey); + + // Remove spaces + auto it = std::remove(uniqueId.begin(), uniqueId.end(), ' '); + uniqueId.erase(it , uniqueId.end()); + + // Remove '/' + it = std::remove(uniqueId.begin(), uniqueId.end(), '/'); + uniqueId.erase(it , uniqueId.end()); + + return uniqueId; +} + +void Configuration::setGravityVector(const std::array& gravityVector) +{ + m_gravityVector = gravityVector; +} + +// GET METHODS +// =========== + +const std::string& Configuration::getRobotName() const +{ + return m_robotName; +} + +const std::string& Configuration::getUrdfFile() const +{ + return m_urdfFile; +} + +const std::vector& Configuration::getControlledJoints() const +{ + return m_controlledJoints; +} + +const std::vector& Configuration::getControlBoardsNames() const +{ + return m_controlBoardsNames; +} + +const std::string& Configuration::getLocalName() const +{ + return m_localName; +} + +const std::array& Configuration::getGravityVector() const +{ + return m_gravityVector; +} + +const size_t& Configuration::getNumberOfDoFs() const +{ + return m_dofs; +} + +// OTHER METHODS +// ============= + +bool Configuration::isValid() const +{ + bool status = !m_robotName.empty() && + !m_urdfFile.empty() && + !m_localName.empty() && + !m_controlledJoints.empty() && + !m_controlBoardsNames.empty() && + !m_gravityVector.empty() && + m_dofs > 0; + return status; +} + +// OPERATORS OVERLOADING +// ===================== + +bool Configuration::operator==(const Configuration &config) const +{ + return this->m_robotName == config.m_robotName && + this->m_urdfFile == config.m_urdfFile && + this->m_localName == config.m_localName && + this->m_controlledJoints == config.m_controlledJoints && + this->m_controlBoardsNames == config.m_controlBoardsNames && + this->m_gravityVector == config.m_gravityVector && + this->m_dofs == config.m_dofs; +} diff --git a/toolbox/src/base/Error.cpp b/toolbox/src/base/Error.cpp deleted file mode 100644 index e69de29bb..000000000 diff --git a/toolbox/src/base/Log.cpp b/toolbox/src/base/Log.cpp new file mode 100644 index 000000000..47769393c --- /dev/null +++ b/toolbox/src/base/Log.cpp @@ -0,0 +1,88 @@ +#include "Log.h" +#include +#include + +using namespace wbt; + +Log& Log::getSingleton() +{ + static Log logInstance; + return logInstance; +} + +void Log::error(const std::string& errorMessage) +{ + errors.push_back(errorMessage); +} + +void Log::warning(const std::string& warningMessage) +{ + warnings.push_back(warningMessage); +} + +void Log::errorAppend(const std::string& errorMessage) +{ + if (errors.empty()) { + error(errorMessage); + return; + } + errors.back() += errorMessage; +} + +void Log::warningAppend(const std::string& warningMessage) +{ + if (warnings.empty()) { + warning(warningMessage); + } + warnings.back() += warningMessage; +} + +void Log::setPrefix(const std::string& prefixMessage) +{ + prefix = prefixMessage; +} + +void Log::resetPrefix() +{ + prefix.clear(); +} + +std::string Log::serializeVectorString(std::vector v, const std::string& prefix) +{ + std::stringstream output; + std::ostream_iterator output_iterator(output, "\n"); + std::copy(v.begin(), v.end(), output_iterator); + + if (prefix.empty()) { + return output.str(); + } + else { + return prefix + "\n" + output.str(); + } +} + +std::string Log::getErrors() const +{ + return serializeVectorString(errors, prefix); +} + +std::string Log::getWarnings() const +{ + return serializeVectorString(warnings, prefix); +} + +void Log::clearWarnings() +{ + warnings.clear(); +} + +void Log::clearErrors() +{ + errors.clear(); +} + +void Log::clear() +{ + clearErrors(); + clearWarnings(); +} diff --git a/toolbox/src/base/RobotInterface.cpp b/toolbox/src/base/RobotInterface.cpp new file mode 100644 index 000000000..30662d2e4 --- /dev/null +++ b/toolbox/src/base/RobotInterface.cpp @@ -0,0 +1,546 @@ +#include "RobotInterface.h" +#include "Log.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace wbt { + // The declaration of the following template specializations are required only by GCC + using namespace yarp::dev; + template <> bool RobotInterface::getInterface(IControlMode2*& interface); + template <> bool RobotInterface::getInterface(IPositionControl*& interface); + template <> bool RobotInterface::getInterface(IPositionDirect*& interface); + template <> bool RobotInterface::getInterface(IVelocityControl*& interface); + template <> bool RobotInterface::getInterface(ITorqueControl*& interface); + template <> bool RobotInterface::getInterface(IPWMControl*& interface); + template <> bool RobotInterface::getInterface(ICurrentControl*& interface); + template <> bool RobotInterface::getInterface(IEncoders*& interface); + template <> bool RobotInterface::getInterface(IControlLimits2*& interface); + template <> bool RobotInterface::getInterface(IPidControl*& interface); +} + +using namespace wbt; + +// CONSTRUCTOR / DESTRUCTOR +// ======================== + + +RobotInterface::RobotInterface(const wbt::Configuration& c) +: m_config(c) +, m_robotDeviceCounter(0) +{} + +RobotInterface::~RobotInterface() +{ + // Asserts for debugging purpose. + + // - 1 if at least one block asked the model. At this point only the shared_ptr + // of m_kinDynComp should be still alive (--> 1) + // - 0 if no block asked for the model. m_kinDynComp was never allocated. + assert(m_kinDynComp.use_count() <= 1); + + // m_robotDevice should be destroyed by the last releaseCB() + assert(!m_robotDevice); + assert(m_robotDeviceCounter == 0); +} + +bool RobotInterface::getSingleControlBoard(const std::string& remoteName, std::unique_ptr& controlBoard) { + // Configure the single control board + yarp::os::Property options; + options.clear(); + options.put("device", "remote_controlboard"); + options.put("remote", remoteName); + options.put("local", m_config.getLocalName() + "/CBtmp"); + options.put("writeStrict", "on"); + + // Initialize the device + controlBoard = std::unique_ptr(new yarp::dev::PolyDriver()); + if (!controlBoard) { + Log::getSingleton().error("Failed to retain the RemoteControlBoard "); + Log::getSingleton().errorAppend("used for mapping iDynTree - YARP DoFs."); + return false; + } + + // Try to open the control board + if (!controlBoard->open(options) || !controlBoard->isValid()) { + Log::getSingleton().error("Unable to open RemoteControlBoard " + remoteName); + return false; + } + + return true; +} + +bool RobotInterface::mapDoFs() +{ + // Initialize the network + yarp::os::Network::init(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } + + std::vector> iAxisInfos; + + for (unsigned cbNum = 0; cbNum < m_config.getControlBoardsNames().size(); ++cbNum) { + + std::unique_ptr controlBoard; + const std::string prefix = "/" + m_config.getRobotName() + "/"; + const std::string remoteName = prefix + m_config.getControlBoardsNames().at(cbNum); + + if (!getSingleControlBoard(remoteName, controlBoard)) { + return false; + } + + // Get an IAxisInfo object from the interface + std::unique_ptr iAxisInfo; + yarp::dev::IAxisInfo* iAxisInfoPtr = iAxisInfo.get(); + controlBoard->view(iAxisInfoPtr); + if (!iAxisInfoPtr) { + Log::getSingleton().error("Unable to open IAxisInfo from " + remoteName); + return false; + } + + // Get an IEncoders object from the interface + // This is used to get how many joints the control board contains + std::unique_ptr iEnc; + yarp::dev::IEncoders* iEncPtr = iEnc.get(); + controlBoard->view(iEncPtr); + int numAxes; + if (!iEncPtr || !iEncPtr->getAxes(&numAxes)) { + Log::getSingleton().error("Unable to open IEncoders from " + remoteName); + return false; + } + + int found = -1; + // Iterate all the joints in the selected Control Board + for (unsigned axis = 0; axis < numAxes; ++axis) { + std::string axisName; + if (!iAxisInfoPtr->getAxisName(axis, axisName)) { + Log::getSingleton().error("Unable to get AxisName from " + remoteName); + return false; + } + // Look if axisName is a controlledJoint + for (const auto& controlledJoint : m_config.getControlledJoints()) { + if (controlledJoint == axisName) { + found++; + // Get the iDynTree index from the model + const auto& kinDynComp = getKinDynComputations(); + if (!kinDynComp) { + Log::getSingleton().error("Failed to get KinDynComputations."); + return false; + } + const auto& model = kinDynComp->model(); + iDynTree::JointIndex iDynJointIdx = model.getJointIndex(axisName); + if (iDynJointIdx == iDynTree::JOINT_INVALID_INDEX) { + Log::getSingleton().error("Joint " + axisName + " exists in the " + + remoteName + "control board but not in the model."); + return false; + } + // If this is the first entry to add, allocate the objects + if (!m_jointsMapIndex) { + m_jointsMapIndex = std::make_shared(); + } + if (!m_jointsMapString) { + m_jointsMapString = std::make_shared(); + } + if (!m_controlledJointsMapCB) { + m_controlledJointsMapCB = std::make_shared(); + } + if (!m_controlBoardIdxLimit) { + m_controlBoardIdxLimit = std::make_shared(); + } + // Create a new entry in the map objects + m_jointsMapString->insert(std::make_pair(controlledJoint, std::make_pair(cbNum, axis))); + m_jointsMapIndex->insert(std::make_pair(static_cast(iDynJointIdx), + std::make_pair(cbNum, axis))); + m_controlledJointsMapCB->insert(std::make_pair(controlledJoint, found)); + (*m_controlBoardIdxLimit)[cbNum] = found + 1; + break; + } + } + + } + + // Notify that the control board just checked is not used by any joint + // of the controlledJoints list + if (found < 0) { + Log::getSingleton().warning("No controlled joints found in " + + m_config.getControlBoardsNames().at(cbNum) + + " control board. It might be unused."); + } + + // Close the ControlBoard device + if (!controlBoard->close()) { + Log::getSingleton().error("Unable to close the interface of the Control Board."); + return false; + } + } + + // Initialize the network + yarp::os::Network::fini(); + + return true; +} + +// GET METHODS +// =========== + +const wbt::Configuration& RobotInterface::getConfiguration() const +{ + return m_config; +} + +const std::shared_ptr RobotInterface::getJointsMapString() +{ + if (!m_jointsMapString || m_jointsMapString->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create the joint maps."); + return nullptr; + } + } + + return m_jointsMapString; +} + +const std::shared_ptr RobotInterface::getJointsMapIndex() +{ + if (!m_jointsMapIndex || m_jointsMapIndex->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create the joint maps."); + return nullptr; + } + } + + assert (m_jointsMapIndex); + return m_jointsMapIndex; +} + +const std::shared_ptr RobotInterface::getControlledJointsMapCB() +{ + if (!m_controlledJointsMapCB || m_controlledJointsMapCB->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create joint maps."); + return nullptr; + } + } + + assert (m_controlledJointsMapCB); + return m_controlledJointsMapCB; +} + +const std::shared_ptr RobotInterface::getControlBoardIdxLimit() +{ + if (!m_controlBoardIdxLimit || m_controlBoardIdxLimit->empty()) { + if (!mapDoFs()) { + Log::getSingleton().error("Failed to create joint maps."); + return nullptr; + } + } + + assert (m_controlBoardIdxLimit); + return m_controlBoardIdxLimit; +} + +const std::shared_ptr RobotInterface::getKinDynComputations() +{ + if (m_kinDynComp) { + return m_kinDynComp; + } + + // Otherwise, initialize a new object + if (!initializeModel()) { + Log::getSingleton().error("Failed to initialize the KinDynComputations object."); + // Return an empty shared_ptr (implicitly initialized) + return nullptr; + } + + return m_kinDynComp; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IControlMode2*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iControlMode2); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IPositionControl*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPositionControl); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IPositionDirect*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPositionDirect); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IVelocityControl*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iVelocityControl); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::ITorqueControl*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iTorqueControl); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IPWMControl*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPWMControl); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::ICurrentControl*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iCurrentControl); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IEncoders*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iEncoders); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IControlLimits2*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iControlLimits2); + return interface; +} + +template <> +bool RobotInterface::getInterface(yarp::dev::IPidControl*& interface) +{ + interface = getInterfaceFromTemplate(m_yarpInterfaces.iPidControl); + return interface; +} + +// LAZY EVALUATION +// =============== + +bool RobotInterface::retainRemoteControlBoardRemapper() +{ + if (m_robotDeviceCounter > 0) { + m_robotDeviceCounter++; + return true; + } + + assert(!m_robotDevice); + if (m_robotDevice) { + m_robotDevice.reset(); + } + + if (!initializeRemoteControlBoardRemapper()) { + Log::getSingleton().error("First initialization of the RemoteControlBoardRemapper failed."); + return false; + } + + m_robotDeviceCounter++; + return true; +} + +bool RobotInterface::releaseRemoteControlBoardRemapper() +{ + // The RemoteControlBoardRemapper has not been used + if (m_robotDeviceCounter == 0) { + return true; + } + + // If there are at most 2 blocks with th CB still used + if (m_robotDeviceCounter > 1) { + m_robotDeviceCounter--; + return true; + } + + // This should be executed by the last block which uses CB (m_robotDeviceCounter=1) + assert(m_robotDevice); + if (m_robotDevice) { + // Set to zero all the pointers to the interfaces + m_yarpInterfaces.iControlMode2 = nullptr; + m_yarpInterfaces.iPositionControl = nullptr; + m_yarpInterfaces.iPositionDirect = nullptr; + m_yarpInterfaces.iVelocityControl = nullptr; + m_yarpInterfaces.iTorqueControl = nullptr; + m_yarpInterfaces.iPWMControl = nullptr; + m_yarpInterfaces.iCurrentControl = nullptr; + m_yarpInterfaces.iEncoders = nullptr; + m_yarpInterfaces.iControlLimits2 = nullptr; + m_yarpInterfaces.iPidControl = nullptr; + // Close the device (which deletes the interfaces it allocated) + m_robotDevice->close(); + // Free the object + m_robotDevice.reset(); + } + + // Initialize the network + yarp::os::Network::init(); + + m_robotDeviceCounter = 0; + return true; +} + +// INITIALIZATION HELPERS +// ====================== + +bool RobotInterface::initializeModel() +{ + assert (!m_kinDynComp); + + // Allocate the object + m_kinDynComp = std::make_shared(); + if (!m_kinDynComp) return false; + + // Explicitly set the velocity representation + m_kinDynComp->setFrameVelocityRepresentation(iDynTree::MIXED_REPRESENTATION); + + // Use RF to load the urdf file + // ---------------------------- + + // Initialize RF + // Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 + using namespace yarp::os; + Network network; + ResourceFinder& rf = ResourceFinder::getResourceFinderSingleton(); + rf.configure(0, 0); + + // Get the absolute path of the urdf file + std::string urdf_file = getConfiguration().getUrdfFile(); + std::string urdf_file_path = rf.findFile(urdf_file.c_str()); + + // Load the reduced model into KinDynComputations + // ---------------------------------------------- + + // Load the joint list + std::vector controlledJoints = getConfiguration().getControlledJoints(); + + // Use ModelLoader to load the reduced model + iDynTree::ModelLoader mdlLoader; + if (!mdlLoader.loadReducedModelFromFile(urdf_file_path, controlledJoints)) { + Log::getSingleton().error("ToolboxSingleton: impossible to load " + urdf_file + "."); + Log::getSingleton().errorAppend("\nPossible causes: file not found, or the joint "); + Log::getSingleton().errorAppend("list contains an entry not present in the urdf model."); + return false; + } + + // Add the loaded model to the KinDynComputations object + return m_kinDynComp->loadRobotModel(mdlLoader.model()); +} + +bool RobotInterface::initializeRemoteControlBoardRemapper() +{ + // Initialize the network + yarp::os::Network::init(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } + + // Object where the RemoteControlBoardRemapper options will be stored + yarp::os::Property options; + + // Name of the device + options.put("device", "remotecontrolboardremapper"); + + // Controlled joints (axes) + yarp::os::Bottle axesNames; + yarp::os::Bottle& axesList = axesNames.addList(); + for (auto axis : m_config.getControlledJoints()) { + axesList.addString(axis); + } + options.put("axesNames",axesNames.get(0)); + + // ControlBoard names + yarp::os::Bottle remoteControlBoards; + yarp::os::Bottle& remoteControlBoardsList = remoteControlBoards.addList(); + for (auto cb : m_config.getControlBoardsNames()) { + remoteControlBoardsList.addString("/" + m_config.getRobotName() + "/" + cb); + } + options.put("remoteControlBoards", remoteControlBoards.get(0)); + + // Prefix of the openened ports + // In this case appending the unique id is necessary, since multiple configuration can + // share some ControlBoard in their RemoteControlBoardRemappers. In this case, it is not + // possible using the same prefix for all the RemoteControlBoardRemapper devices. + options.put("localPortPrefix", m_config.getLocalName() + "/" + m_config.getUniqueId()); + + // Misc options + yarp::os::Property& remoteCBOpts = options.addGroup("REMOTE_CONTROLBOARD_OPTIONS"); + remoteCBOpts.put("writeStrict", "on"); + + // If resources have been properly cleaned, there should be no allocated device. + // However, if blocks fail and they don't terminate, the state of the singleton + // could be not clean. + if (m_robotDevice) { + // Force the release + m_robotDeviceCounter = 1; + Log::getSingleton().warning("The ToolboxSingleton state is dirty. Trying to clean the state before proceeding."); + if (!releaseRemoteControlBoardRemapper()) { + Log::getSingleton().error("Failed to force the release of the RemoteControlBoardRemapper. "); + return false; + } + } + + // Allocate the interface driver + m_robotDevice = std::unique_ptr(new yarp::dev::PolyDriver()); + + if (!m_robotDevice) { + Log::getSingleton().error("Failed to instantiante an empty PolyDriver class."); + return false; + } + + // Open the interface driver + if (!m_robotDevice->open(options) && !m_robotDevice->isValid()) { + // Remove garbage if the opening fails + m_robotDevice.reset(); + Log::getSingleton().error("Failed to open the RemoteControlBoardRemapper with the options passed."); + return false; + } + + return true; +} + +// OTHER METHODS +// ============= + +template +T* RobotInterface::getInterfaceFromTemplate(T*& interface) +{ + if (!interface) { + // Blocks which require the RemoteControlBoardRemapper need to retain / release it + // in their initialization / terminate phase; + // assert(m_robotDevice); + if (!m_robotDevice) { + Log::getSingleton().error("The RemoteControlBoardRemapper has not been initialized. "); + Log::getSingleton().errorAppend("You need to retain the CB device in the initialize() method."); + // Return an empty weak pointer + return nullptr; + } + + // Ask the interface from the device + if (!m_robotDevice->view(interface)) { + // Return an empty weak_ptr + return nullptr; + } + } + return interface; +} diff --git a/toolbox/src/base/Signal.cpp b/toolbox/src/base/Signal.cpp index de33aaf15..b55bb425f 100644 --- a/toolbox/src/base/Signal.cpp +++ b/toolbox/src/base/Signal.cpp @@ -1,264 +1,189 @@ #include "Signal.h" -#include +using namespace wbt; -namespace wbt { - - - Signal::Signal() {} +Signal::~Signal() +{ + deleteBuffer(); +} - void Signal::initSignalType(wbt::PortDataType type, bool constPort) - { - this->portType = type; - this->isConstPort = constPort; +Signal::Signal(const Signal& signal) +: m_width(signal.m_width) +, m_isConst(signal.m_isConst) +, m_portDataType(signal.m_portDataType) +, m_dataFormat(signal.m_dataFormat) +, m_bufferPtr(nullptr) +{ + if (signal.m_bufferPtr) { + switch (signal.m_dataFormat) { + // Just copy the pointer to MATLAB's memory + case CONTIGUOUS_ZEROCOPY: + m_bufferPtr = signal.m_bufferPtr; + break; + // Copy the allocated data + case NONCONTIGUOUS: + case CONTIGUOUS: + allocateBuffer(signal.m_bufferPtr, m_bufferPtr, signal.m_width); + break; + } } +} - void Signal::setContiguousBuffer(void* buffer) - { - contiguousData = buffer; - this->isContiguous = true; - } - void Signal::setContiguousBuffer(const void* buffer) - { - contiguousData = const_cast(buffer); - this->isContiguous = true; - } +Signal::Signal(const SignalDataFormat& dataFormat, + const PortDataType& dataType, + const bool& isConst) +: m_isConst(isConst) +, m_portDataType(dataType) +, m_dataFormat(dataFormat) +, m_bufferPtr(nullptr) +{} + +Signal::Signal(Signal&& other) +: m_width(other.m_width) +, m_isConst(other.m_isConst) +, m_portDataType(other.m_portDataType) +, m_dataFormat(other.m_dataFormat) +, m_bufferPtr(other.m_bufferPtr) +{ + other.m_width = 0; + other.m_bufferPtr = nullptr; +} - void Signal::setNonContiguousBuffer(void** buffer) - { - nonContiguousData = buffer; - this->isContiguous = false; +void Signal::allocateBuffer(const void* const bufferInput, void*& bufferOutput, unsigned length) +{ + // TODO: Implement other PortDataType + switch (m_portDataType) { + case PortDataTypeDouble: { + // Allocate the array + bufferOutput = static_cast(new double[m_width]); + // Cast to double + const double* const bufferInputDouble = static_cast(bufferInput); + double* bufferOutputDouble = static_cast(bufferOutput); + // Copy data + std::copy(bufferInputDouble, bufferInputDouble + length, bufferOutputDouble); + return; + } + default: + return; } +} - void Signal::setNonContiguousBuffer(const void* const * buffer) - { - nonContiguousData = const_cast(buffer); - this->isContiguous = false; +void Signal::deleteBuffer() +{ + if (m_dataFormat == CONTIGUOUS_ZEROCOPY || !m_bufferPtr) { + return; } - - const Data Signal::get(unsigned index) const - { - Data data; - switch (portType) { - case PortDataTypeDouble: - data.doubleData(isContiguous ? ((double*)contiguousData)[index] : *((double **)nonContiguousData)[index]); - break; - case PortDataTypeSingle: - data.floatData(isContiguous ? ((float*)contiguousData)[index] : *((float **)nonContiguousData)[index]); - break; - case PortDataTypeInt8: - data.int8Data(isContiguous ? ((int8_t*)contiguousData)[index] : *((int8_t **)nonContiguousData)[index]); - break; - case PortDataTypeUInt8: - data.uint8Data(isContiguous ? ((uint8_t*)contiguousData)[index] : *((uint8_t **)nonContiguousData)[index]); - break; - case PortDataTypeInt16: - data.int8Data(isContiguous ? ((int16_t*)contiguousData)[index] : *((int16_t **)nonContiguousData)[index]); - break; - case PortDataTypeUInt16: - data.uint8Data(isContiguous ? ((uint16_t*)contiguousData)[index] : *((uint16_t **)nonContiguousData)[index]); - break; - case PortDataTypeInt32: - data.int8Data(isContiguous ? ((int32_t*)contiguousData)[index] : *((int32_t **)nonContiguousData)[index]); - break; - case PortDataTypeUInt32: - data.uint8Data(isContiguous ? ((uint32_t*)contiguousData)[index] : *((uint32_t **)nonContiguousData)[index]); - break; - case PortDataTypeBoolean: - data.booleanData(isContiguous ? ((bool*)contiguousData)[index] : *((bool **)nonContiguousData)[index]); - } - return data; + // TODO: Implement other PortDataType + switch (m_portDataType) { + case PortDataTypeDouble: + delete static_cast(m_bufferPtr); + m_bufferPtr = nullptr; + return; + default: + return; } +} - void* Signal::getContiguousBuffer() - { - if (!isContiguous) return 0; - return this->contiguousData; +bool Signal::initializeBufferFromContiguousZeroCopy(const void* buffer) +{ + if (m_dataFormat != CONTIGUOUS_ZEROCOPY) { + return false; } - //the missing are cast - void Signal::set(unsigned index, double data) - { - if (isConstPort) return; + m_bufferPtr = const_cast(buffer); + return true; +} - switch (portType) { - case PortDataTypeDouble: - { - double *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeSingle: - { - float *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; - } +bool Signal::initializeBufferFromContiguous(const void* buffer) +{ + if (m_dataFormat != CONTIGUOUS || + m_width <= 0) { + return false; } - void Signal::setBuffer(const double *data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = 0; - const void * address = data; - - switch (portType) { - case PortDataTypeDouble: - { - dataSize = sizeof(double); - address = data + startIndex; - break; - } - case PortDataTypeSingle: - { - dataSize = sizeof(float); - address = ((float*)data) + startIndex; - break; - } - default: - break; - } - - memcpy(contiguousData, address, dataSize * length); + if (m_portDataType == PortDataTypeDouble) { + // Allocate a new vector to store data from the non-contiguous signal + m_bufferPtr = static_cast(new double[m_width]); + const double* bufferInputDouble = static_cast(buffer); + double* bufferOutputDouble = static_cast(m_bufferPtr); + // Copy data from the input memory location + std::copy(bufferInputDouble, bufferInputDouble + m_width, bufferOutputDouble); } + return true; +} - void Signal::set(unsigned index, int32_t data) - { - //signed integer function - switch (portType) { - case PortDataTypeInt32: - { - int32_t *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeInt16: - { - int16_t *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeInt8: - { - int8_t *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; - } +bool Signal::initializeBufferFromNonContiguous(const void* const* bufferPtrs) +{ + if (m_dataFormat != NONCONTIGUOUS || + m_width <= 0) { + return false; } - void Signal::setBuffer(const int32_t *data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = 0; - const void * address = data; + if (m_portDataType == PortDataTypeDouble) { + // Allocate a new vector to store data from the non-contiguous signal + m_bufferPtr = static_cast(new double[m_width]); + double* bufferPtrDouble = static_cast(m_bufferPtr); - switch (portType) { - case PortDataTypeInt32: - { - dataSize = sizeof(int32_t); - address = ((int32_t*)data) + startIndex; - break; - } - case PortDataTypeInt16: - { - dataSize = sizeof(int16_t); - address = ((int16_t*)data) + startIndex; - break; - } - case PortDataTypeInt8: - { - dataSize = sizeof(int8_t); - address = ((int8_t*)data) + startIndex; - break; - } - default: - break; + // Copy data from MATLAB's memory to the Signal object + for (auto i = 0; i < m_width; ++i) { + const double* valuePtr = static_cast(*bufferPtrs); + bufferPtrDouble[i] = valuePtr[i]; } - - memcpy(contiguousData, address, dataSize * length); } + return true; +} - void Signal::set(unsigned index, uint32_t data) - { - //signed integer function - switch (portType) { - case PortDataTypeUInt32: - { - uint32_t *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeUInt16: - { - uint16_t *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - case PortDataTypeUInt8: - { - uint8_t *buffer = static_cast(contiguousData); - buffer[index] = data; - break; - } - default: - break; - } - } +void Signal::setWidth(const unsigned& width) +{ + m_width = width; +} - void Signal::setBuffer(const uint32_t *data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = 0; - const void * address = data; +unsigned Signal::getWidth() const +{ + return m_width; +} - switch (portType) { - case PortDataTypeUInt32: - { - dataSize = sizeof(uint32_t); - address = ((uint32_t*)data) + startIndex; - break; - } - case PortDataTypeUInt16: - { - dataSize = sizeof(uint16_t); - address = ((uint16_t*)data) + startIndex; - break; - } - case PortDataTypeUInt8: - { - dataSize = sizeof(uint8_t); - address = ((uint8_t*)data) + startIndex; - break; - } - default: - break; - } +PortDataType Signal::getPortDataType() const +{ + return m_portDataType; +} - memcpy(contiguousData, address, dataSize * length); - } +bool Signal::isConst() const +{ + return m_isConst; +} - void Signal::set(unsigned index, bool data) - { - bool *buffer = static_cast(contiguousData); - buffer[index] = data; - } - void Signal::setBuffer(const bool *data, const unsigned length, unsigned startIndex) - { - if (isConstPort) return; - unsigned dataSize = sizeof(bool); - const void * address = ((bool*)data) + startIndex; +SignalDataFormat Signal::getDataFormat() const +{ + return m_dataFormat; +} - memcpy(contiguousData, address, dataSize * length); +bool Signal::set(const unsigned& index, const double& data) +{ + if (m_isConst || m_width <= index) { + return false; } - + // TODO: Implement other PortDataType + switch (m_portDataType) { + case PortDataTypeDouble: + { + double* buffer = static_cast(m_bufferPtr); + buffer[index] = data; + break; + } + case PortDataTypeSingle: + { + float* buffer = static_cast(m_bufferPtr); + buffer[index] = data; + break; + } + default: + return false; + break; + } + return true; } diff --git a/toolbox/src/base/SimulinkBlockInformation.cpp b/toolbox/src/base/SimulinkBlockInformation.cpp index 85dc6593d..f8825d0c8 100644 --- a/toolbox/src/base/SimulinkBlockInformation.cpp +++ b/toolbox/src/base/SimulinkBlockInformation.cpp @@ -1,186 +1,269 @@ #include "SimulinkBlockInformation.h" - #include "Signal.h" - +#include "MxAnyType.h" #include "simstruc.h" +#include +#include -namespace wbt { +using namespace wbt; - SimulinkBlockInformation::SimulinkBlockInformation(SimStruct *S) - : simstruct(S) {} +SimulinkBlockInformation::SimulinkBlockInformation(SimStruct* S) +: simstruct(S) +{} - SimulinkBlockInformation::~SimulinkBlockInformation() {} - - bool SimulinkBlockInformation::optionFromKey(const std::string& key, Data &option) const - { - if (key == wbt::BlockOptionPrioritizeOrder) { - option.uint32Data(SS_OPTION_PLACE_ASAP); - return true; - } +// BLOCK OPTIONS METHODS +// ===================== - return false; +bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& option) const +{ + if (key == wbt::BlockOptionPrioritizeOrder) { + option = SS_OPTION_PLACE_ASAP; + return true; } - //Parameters methods - bool SimulinkBlockInformation::getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) - { - int_T buflen, status; - char *buffer = NULL; - - //robot name - buflen = (1 + mxGetN(ssGetSFcnParam(simstruct, parameterIndex))) * sizeof(mxChar); - buffer = static_cast(mxMalloc(buflen)); - status = mxGetString((ssGetSFcnParam(simstruct, parameterIndex)), buffer, buflen); - if (status) { - return false; - } - stringParameter = buffer; - mxFree(buffer); buffer = NULL; + return false; +} + +// PARAMETERS METHODS +// ================== + +bool SimulinkBlockInformation::getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asString(stringParameter); +} + +bool SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex, double& value) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asDouble(value); +} + +bool SimulinkBlockInformation::getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const +{ + double tmpValue = 0; + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + + // The Simulink mask often doesn't store boolean data from the mask as bool but as double. + // Calling asBool() will fail in this case. If this happens, asDouble() is used as fallback. + if (MxAnyType(blockParam).asBool(value)) { return true; } - - Data SimulinkBlockInformation::getScalarParameterAtIndex(unsigned parameterIndex) - { - Data data; - data.doubleData(mxGetScalar(ssGetSFcnParam(simstruct, parameterIndex))); - return data; + else if (MxAnyType(blockParam).asDouble(tmpValue)) { + value = static_cast(tmpValue); + return true; } + return MxAnyType(blockParam).asBool(value); +} - //Port information methods - bool SimulinkBlockInformation::setNumberOfInputPorts(unsigned numberOfPorts) - { - return ssSetNumInputPorts(simstruct, numberOfPorts); - } +bool SimulinkBlockInformation::getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asAnyStruct(map); +} - bool SimulinkBlockInformation::setNumberOfOuputPorts(unsigned numberOfPorts) - { - return ssSetNumOutputPorts(simstruct, numberOfPorts); - } - bool SimulinkBlockInformation::setInputPortVectorSize(unsigned portNumber, int portSize) - { - if (portSize == -1) portSize = DYNAMICALLY_SIZED; - return ssSetInputPortVectorDimension(simstruct, portNumber, portSize); - } +bool SimulinkBlockInformation::getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asVectorDouble(vec); +} - bool SimulinkBlockInformation::setInputPortMatrixSize(unsigned portNumber, int rows, int columns) - { - if (rows == -1) rows = DYNAMICALLY_SIZED; - if (columns == -1) columns = DYNAMICALLY_SIZED; - return ssSetInputPortMatrixDimensions(simstruct, portNumber, rows, columns); - } +// PORT INFORMATION SETTERS +// ======================== + +bool SimulinkBlockInformation::setNumberOfInputPorts(unsigned numberOfPorts) +{ + return ssSetNumInputPorts(simstruct, numberOfPorts); +} + +bool SimulinkBlockInformation::setNumberOfOutputPorts(unsigned numberOfPorts) +{ + return ssSetNumOutputPorts(simstruct, numberOfPorts); +} + +bool SimulinkBlockInformation::setInputPortVectorSize(unsigned portNumber, int portSize) +{ + if (portSize == -1) portSize = DYNAMICALLY_SIZED; + return ssSetInputPortVectorDimension(simstruct, portNumber, portSize); +} + +bool SimulinkBlockInformation::setInputPortMatrixSize(unsigned portNumber, int rows, int columns) +{ + if (rows == -1) rows = DYNAMICALLY_SIZED; + if (columns == -1) columns = DYNAMICALLY_SIZED; + return ssSetInputPortMatrixDimensions(simstruct, portNumber, rows, columns); +} + +bool SimulinkBlockInformation::setOutputPortVectorSize(unsigned portNumber, int portSize) +{ + if (portSize == -1) portSize = DYNAMICALLY_SIZED; + return ssSetOutputPortVectorDimension(simstruct, portNumber, portSize); +} + +bool SimulinkBlockInformation::setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) +{ + if (rows == -1) rows = DYNAMICALLY_SIZED; + if (columns == -1) columns = DYNAMICALLY_SIZED; + return ssSetOutputPortMatrixDimensions(simstruct, portNumber, rows, columns); +} + +bool SimulinkBlockInformation::setInputPortType(unsigned portNumber, PortDataType portType) +{ + ssSetInputPortDirectFeedThrough(simstruct, portNumber, 1); + ssSetInputPortDataType(simstruct, portNumber, mapSimulinkToPortType(portType)); + return true; +} + +bool SimulinkBlockInformation::setOutputPortType(unsigned portNumber, PortDataType portType) +{ + ssSetOutputPortDataType(simstruct, portNumber, mapSimulinkToPortType(portType)); + return true; +} + +// PORT INFORMATION GETTERS +// ======================== + +unsigned SimulinkBlockInformation::getInputPortWidth(unsigned portNumber) const +{ + return ssGetInputPortWidth(simstruct, portNumber); +} - bool SimulinkBlockInformation::setOutputPortVectorSize(unsigned portNumber, int portSize) - { - if (portSize == -1) portSize = DYNAMICALLY_SIZED; - return ssSetOutputPortVectorDimension(simstruct, portNumber, portSize); +unsigned SimulinkBlockInformation::getOutputPortWidth(unsigned portNumber) const +{ + return ssGetOutputPortWidth(simstruct, portNumber); +} + +wbt::Signal SimulinkBlockInformation::getInputPortSignal(unsigned portNumber, int portWidth) const +{ + // Read if the signal is contiguous or non-contiguous + boolean_T isContiguous = ssGetInputPortRequiredContiguous(simstruct, portNumber); + SignalDataFormat sigDataFormat = isContiguous ? CONTIGUOUS_ZEROCOPY : NONCONTIGUOUS; + + // Check if the signal is dynamically sized (which means that the dimension + // cannot be read) + bool isDynamicallySized = (ssGetInputPortWidth(simstruct, portNumber) == DYNAMICALLY_SIZED); + + // Note that if the signal is DYNAMICALLY_SIZED (-1), portWidth is necessary + if (isDynamicallySized && portWidth == -1) { + return Signal(); } - bool SimulinkBlockInformation::setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) - { - if (rows == -1) rows = DYNAMICALLY_SIZED; - if (columns == -1) columns = DYNAMICALLY_SIZED; - return ssSetOutputPortMatrixDimensions(simstruct, portNumber, rows, columns); + // Read the width of the signal if it is not provided as input and the signal is not + // dynamically sized + if (!isDynamicallySized && portWidth == -1) { + portWidth = ssGetInputPortWidth(simstruct, portNumber); } - bool SimulinkBlockInformation::setInputPortType(unsigned portNumber, PortDataType portType) - { - //for now force Direct feedthrough.. If needed create a separate method - ssSetInputPortDirectFeedThrough(simstruct, portNumber, 1); - int matlabDataType = -1; - switch (portType) { - case PortDataTypeDouble: - matlabDataType = SS_DOUBLE; - break; - case PortDataTypeSingle: - matlabDataType = SS_SINGLE; - break; - case PortDataTypeInt8: - matlabDataType = SS_INT8; - break; - case PortDataTypeUInt8: - matlabDataType = SS_UINT8; - break; - case PortDataTypeInt16: - matlabDataType = SS_INT16; - break; - case PortDataTypeUInt16: - matlabDataType = SS_UINT16; - break; - case PortDataTypeInt32: - matlabDataType = SS_INT32; - break; - case PortDataTypeUInt32: - matlabDataType = SS_UINT32; - break; - case PortDataTypeBoolean: - matlabDataType = SS_BOOLEAN; - break; + // Get the data type of the Signal if set (default: double) + DTypeId dataType = ssGetInputPortDataType(simstruct, portNumber); + + switch (sigDataFormat) { + case CONTIGUOUS_ZEROCOPY: { + // Initialize the signal + bool isConstPort = true; + Signal signal(CONTIGUOUS_ZEROCOPY, mapSimulinkToPortType(dataType), isConstPort); + signal.setWidth(portWidth); + // Initialize signal's data + if (!signal.initializeBufferFromContiguousZeroCopy(ssGetInputPortSignal(simstruct, portNumber))) { + return Signal(); + } + return signal; + } + case NONCONTIGUOUS: { + // Initialize the signal + bool isConstPort = true; + Signal signal(NONCONTIGUOUS, mapSimulinkToPortType(dataType), isConstPort); + signal.setWidth(portWidth); + // Initialize signal's data + InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, portNumber); + if (!signal.initializeBufferFromNonContiguous(static_cast(port))) { + return Signal(); + } + return signal; } - return ssSetInputPortDataType(simstruct, portNumber, matlabDataType); + default: + return Signal(); } +} - bool SimulinkBlockInformation::setOutputPortType(unsigned portNumber, PortDataType portType) - { - int matlabDataType = -1; - switch (portType) { - case PortDataTypeDouble: - matlabDataType = SS_DOUBLE; - break; - case PortDataTypeSingle: - matlabDataType = SS_SINGLE; - break; - case PortDataTypeInt8: - matlabDataType = SS_INT8; - break; - case PortDataTypeUInt8: - matlabDataType = SS_UINT8; - break; - case PortDataTypeInt16: - matlabDataType = SS_INT16; - break; - case PortDataTypeUInt16: - matlabDataType = SS_UINT16; - break; - case PortDataTypeInt32: - matlabDataType = SS_INT32; - break; - case PortDataTypeUInt32: - matlabDataType = SS_UINT32; - break; - case PortDataTypeBoolean: - matlabDataType = SS_BOOLEAN; - break; - } - return ssSetOutputPortDataType(simstruct, portNumber, matlabDataType); +wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber, int portWidth) const +{ + // Check if the signal is dynamically sized (which means that the dimension + // cannot be read) + bool isDynamicallySized = (ssGetOutputPortWidth(simstruct, portNumber) == DYNAMICALLY_SIZED); + + // Note that if the signal is DYNAMICALLY_SIZED (-1), portWidth is necessary + if (isDynamicallySized && portWidth == -1) { + return Signal(); } - //Port data - unsigned SimulinkBlockInformation::getInputPortWidth(unsigned portNumber) - { - return ssGetInputPortWidth(simstruct, portNumber); + // Read the width of the signal if it is not provided as input and the signal is not + // dynamically sized + if (!isDynamicallySized && portWidth == -1) { + portWidth = ssGetOutputPortWidth(simstruct, portNumber); } - unsigned SimulinkBlockInformation::getOutputPortWidth(unsigned portNumber) - { - return ssGetOutputPortWidth(simstruct, portNumber); + // Get the data type of the Signal if set (default: double) + DTypeId dataType = ssGetOutputPortDataType(simstruct, portNumber); + + bool isConstPort = false; + Signal signal(CONTIGUOUS_ZEROCOPY, mapSimulinkToPortType(dataType), isConstPort); + signal.setWidth(portWidth); + + if (!signal.initializeBufferFromContiguousZeroCopy(ssGetOutputPortSignal(simstruct, portNumber))) { + return Signal(); } - wbt::Signal SimulinkBlockInformation::getInputPortSignal(unsigned portNumber) - { - Signal signal; - InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, portNumber); - signal.initSignalType(PortDataTypeDouble, true); - signal.setNonContiguousBuffer(port); - return signal; + return signal; +} + +PortDataType SimulinkBlockInformation::mapSimulinkToPortType(const DTypeId& typeId) const +{ + switch (typeId) { + case SS_DOUBLE: + return PortDataTypeDouble; + case SS_SINGLE: + return PortDataTypeSingle; + case SS_INT8: + return PortDataTypeInt8; + case SS_UINT8: + return PortDataTypeUInt8; + case SS_INT16: + return PortDataTypeInt16; + case SS_UINT16: + return PortDataTypeUInt16; + case SS_INT32: + return PortDataTypeInt32; + case SS_UINT32: + return PortDataTypeUInt32; + case SS_BOOLEAN: + return PortDataTypeBoolean; + default: + return PortDataTypeDouble; } +} - wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber) - { - Signal signal; - signal.initSignalType(PortDataTypeDouble, false); - signal.setContiguousBuffer(ssGetOutputPortSignal(simstruct, portNumber)); - return signal; +DTypeId SimulinkBlockInformation::mapPortTypeToSimulink(const PortDataType& dataType) const +{ + switch (dataType) { + case PortDataTypeDouble: + return SS_DOUBLE; + case PortDataTypeSingle: + return SS_SINGLE; + case PortDataTypeInt8: + return SS_INT8; + case PortDataTypeUInt8: + return SS_UINT8; + case PortDataTypeInt16: + return SS_INT16; + case PortDataTypeUInt16: + return SS_UINT16; + case PortDataTypeInt32: + return SS_INT32; + case PortDataTypeUInt32: + return SS_UINT32; + case PortDataTypeBoolean: + return SS_BOOLEAN; } - } diff --git a/toolbox/src/base/ToolboxSingleton.cpp b/toolbox/src/base/ToolboxSingleton.cpp new file mode 100644 index 000000000..9eb76a4f3 --- /dev/null +++ b/toolbox/src/base/ToolboxSingleton.cpp @@ -0,0 +1,110 @@ +#include "ToolboxSingleton.h" + +#include "Log.h" +#include "RobotInterface.h" +#include +#include +#include +#include +#include + +using namespace wbt; + +// CONSTRUCTOR / DESTRUCTOR +// ======================== + +ToolboxSingleton::ToolboxSingleton() {} +ToolboxSingleton::~ToolboxSingleton() {} + +// UTILITIES +// ========= + +int ToolboxSingleton::numberOfDoFs(const std::string& confKey) +{ + if (!isKeyValid(confKey)) + return -1; + else + return m_interfaces[confKey]->getConfiguration().getNumberOfDoFs(); +} + +bool ToolboxSingleton::isKeyValid(const std::string& confKey) +{ + if (m_interfaces.find(confKey) != m_interfaces.end()) { + if (m_interfaces[confKey]) + return true; + else + return false; + } + else { + return false; + } +} + +// GET METHODS +// =========== + +ToolboxSingleton& ToolboxSingleton::sharedInstance() +{ + static ToolboxSingleton instance; + return instance; +} + +const Configuration& ToolboxSingleton::getConfiguration(const std::string& confKey) +{ + return getRobotInterface(confKey)->getConfiguration(); +} + +const std::shared_ptr ToolboxSingleton::getRobotInterface(const std::string& confKey) +{ + if (!isKeyValid(confKey)) { + return nullptr; + } + + return m_interfaces[confKey]; +} + +const std::shared_ptr ToolboxSingleton::getKinDynComputations(const std::string& confKey) +{ + if (!isKeyValid(confKey)) { + return nullptr; + } + + return m_interfaces[confKey]->getKinDynComputations(); +} + +// TOOLBOXSINGLETON CONFIGURATION +// ============================== + +bool ToolboxSingleton::storeConfiguration(const std::string& confKey, const Configuration& config) +{ + if (!config.isValid()) { + return false; + } + + // Add the new Configuration object and override an existing key if it already exist. + // Note: Simulink doesn't flush memory unless Matlab is closed, and static objects stay in memory. + // This may cause problems if the config block's mask is changed after the first compilation. + if (m_interfaces.find(confKey) == m_interfaces.end()) { + m_interfaces[confKey] = std::make_shared(config); + return static_cast(m_interfaces[confKey]); + } + + if (!(m_interfaces[confKey]->getConfiguration() == config)) { + assert(m_interfaces[confKey]); + + // Delete the old configuration (calling the destructor for cleaning garbage) + m_interfaces[confKey].reset(); + m_interfaces.erase(confKey); + + // Allocate a new configuration + m_interfaces[confKey] = std::make_shared(config); + return static_cast(m_interfaces[confKey]); + } + + return true; +} + +void ToolboxSingleton::eraseConfiguration(const std::string& confKey) +{ + m_interfaces.erase(confKey); +} diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp new file mode 100644 index 000000000..c067c80e1 --- /dev/null +++ b/toolbox/src/base/WBBlock.cpp @@ -0,0 +1,348 @@ +#include "WBBlock.h" + +#include "BlockInformation.h" +#include "Log.h" +#include "ToolboxSingleton.h" +#include "Configuration.h" +#include "RobotInterface.h" +#include "Signal.h" +#include "AnyType.h" +#include +#include +#include +#include "iDynTree/KinDynComputations.h" +#include +#include +#include +#include +#include + +using namespace wbt; + +const unsigned WBBlock::ConfigurationParameterIndex = 1; // Struct from Simulink +const unsigned WBBlock::ConfBlockNameParameterIndex = 2; // Absolute name of the block containing the + // configuration + +iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::array& gravity) +: m_gravity(gravity.data(), 3) +, m_jointsVelocity(dofs) +, m_jointsPosition(dofs) +{ + m_jointsPosition.zero(); + m_jointsVelocity.zero(); +} + +bool WBBlock::setRobotState(const wbt::Signal* basePose, + const wbt::Signal* jointsPos, + const wbt::Signal* baseVelocity, + const wbt::Signal* jointsVelocity) +{ + // SAVE THE ROBOT STATE + // ==================== + + using namespace iDynTree; + using namespace Eigen; + typedef Matrix Matrix4dSimulink; + + // Base pose + // --------- + + if (basePose) { + // Get the buffer + double* buffer = basePose->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read the base pose from input port."); + return false; + } + // Fill the data + fromEigen(robotState.m_world_T_base, Matrix4dSimulink(buffer)); + } + + // Joints position + // --------------- + + if (jointsPos) { + // Get the buffer + double* buffer = jointsPos->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read joints positions from input port."); + return false; + } + // Fill the data + for (auto i = 0; i < jointsPos->getWidth(); ++i) { + robotState.m_jointsPosition.setVal(i, buffer[i]); + } + + } + + // Base Velocity + // ------------- + + if (baseVelocity) { + // Get the buffer + double* buffer = baseVelocity->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read the base velocity from input port."); + return false; + } + // Fill the data + robotState.m_baseVelocity = Twist(LinVelocity(buffer, 3), + AngVelocity(buffer+3, 3)); + } + + // Joints velocity + // --------------- + + if (jointsVelocity) { + // Get the buffer + double* buffer = jointsVelocity->getBuffer(); + if (!buffer) { + Log::getSingleton().error("Failed to read joints velocities from input port."); + return false; + } + // Fill the data + for (auto i = 0; i < jointsVelocity->getWidth(); ++i) { + robotState.m_jointsVelocity.setVal(i, buffer[i]); + } + } + + // UPDATE THE IDYNTREE ROBOT STATE WITH NEW DATA + // ============================================= + + const auto& model = getRobotInterface()->getKinDynComputations(); + + if (!model) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } + + bool ok = model->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); + + if (!ok) { + Log::getSingleton().error("Failed to set the iDynTree robot state."); + return false; + } + + return true; +} + +unsigned WBBlock::numberOfParameters() { return 2; } + +bool WBBlock::getWBToolboxParameters(Configuration& config, const BlockInformation* blockInfo) +{ + // Infos + // ===== + // + // For what concern initialization, the callback order is: + // + // -> configureSizeAndPorts() + // -> initialize() + // -> initializeInitialConditions() + // + // Despite class objects after the configureSizeAndPorts() are destroyed and + // reallocated before the initialize(), the ToolboxSingleton remains alive and + // can store from this stage the configuration created by reading the parameters. + // + + // Gather the parameters from the Mask of the Simulink Block + // ========================================================= + + // Get the struct containing the parameters + AnyStruct s; + if (!blockInfo->getStructAtIndex(ConfigurationParameterIndex, s)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; + } + + // Check the existence of all the fields + try { + s.at("RobotName"); + s.at("UrdfFile"); + s.at("ControlledJoints"); + s.at("ControlBoardsNames"); + s.at("LocalName"); + s.at("GravityVector"); + } + catch (const std::out_of_range& e) { + Log::getSingleton().error("Cannot retrieve one or more parameter from config struct."); + return false; + } + + // RobotName + std::string robotName; + if (!s["RobotName"]->asString(robotName)) { + Log::getSingleton().error("Cannot retrieve string from RobotName parameter."); + return false; + } + + // UrdfFile + std::string urdfFile; + if (!s["UrdfFile"]->asString(urdfFile)) { + Log::getSingleton().error("Cannot retrieve string from UrdfFile parameter."); + return false; + } + + // ControlledJoints + AnyCell cellCJ; + if (!s["ControlledJoints"]->asAnyCell(cellCJ)) { + Log::getSingleton().error("Cannot retrieve ControlledJoints parameter."); + return false; + } + + std::vector controlledJoints; + for (auto cell: cellCJ) { + std::string joint; + if (!cell->asString(joint)) { + Log::getSingleton().error("Failed to convert ControlledJoints from cell to strings."); + return false; + } + controlledJoints.push_back(joint); + } + + // ControlBoardsNames + AnyCell cellCBN; + if (!s["ControlBoardsNames"]->asAnyCell(cellCBN)) { + Log::getSingleton().error("Cannot retrieve ControlBoardsNames parameter."); + return false; + } + + std::vector controlBoardsNames; + for (auto cell: cellCBN) { + std::string joint; + if (!cell->asString(joint)) { + Log::getSingleton().error("Failed to convert ControlBoardsNames from cell to string."); + return false; + } + controlBoardsNames.push_back(joint); + } + + // LocalName + std::string localName; + if (!s["LocalName"]->asString(localName)) { + Log::getSingleton().error("Cannot retrieve string from LocalName parameter."); + return false; + } + + std::vector gravityVector; + if (!s["GravityVector"]->asVectorDouble(gravityVector)) { + Log::getSingleton().error("Cannot retrieve vector from GravityVector parameter."); + return false; + } + std::array gravityArray; + for (auto i = 0; i < 3; ++i) { + gravityArray[i] = gravityVector[i]; + } + + // Create the ToolboxConfig object + // =============================== + + // Populate a Property object that stores the input parameters + config.setRobotName(robotName); + config.setUrdfFile(urdfFile); + config.setControlledJoints(controlledJoints); + config.setControlBoardsNames(controlBoardsNames); + config.setLocalName(localName); + config.setGravityVector(gravityArray); + + return true; +} + +const Configuration& WBBlock::getConfiguration() +{ + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + return interface.getConfiguration(confKey); +} + +const std::shared_ptr WBBlock::getRobotInterface() +{ + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + return interface.getRobotInterface(confKey); +} + +bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) +{ + // Remember not allocate / save any data in this function, it won't be persistent. + + // Setup the ToolboxSingleton + // ========================== + + // Setup the DOFs (required by childs for configuring port sizes) + // -------------------------------------------------------------- + + // Initialize the ToolboxSingleton + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + + // Get the absolute name of the block from which the configuration has been retrieved. + // This will be the key of the map containing all the configurations of the ToolboxSingleton. + if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; + } + + // Initialize the configuration block with the unique identifier of the conf block name + Configuration config(confKey); + // Populate the configuration object with data from the Simulink's struct parameter + if (!getWBToolboxParameters(config, blockInfo)) { + return false; + } + + // Check if the configuration is valid + if (!config.isValid()) { + Log::getSingleton().error("The provided configuration is not valid."); + return false; + } + + // Store the configuration inside the ToolboxSingleton + if (!interface.storeConfiguration(confKey, config)) { + Log::getSingleton().error("Failed to configure ToolboxSingleton."); + return false; + } + + // Check if the DoFs are positive (and if the config has been stored successfully) + const Configuration& configFromSingleton = getConfiguration(); + + if (configFromSingleton.getNumberOfDoFs() < 1) { + Log::getSingleton().error("Failed to configure WBBlock. Read 0 DoFs."); + return false; + } + + return true; +} + +bool WBBlock::initialize(const BlockInformation* blockInfo) +{ + // CONFIGURE the ToolboxSingleton + // ============================== + + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + + // Gather again confKey + // Note: despite data for blocks is not persistent between configureSizeAndPorts() + // and initialize() (e.g. confKey), the singleton is not destroyed. + // This means that the stored configurations are still there. + if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { + Log::getSingleton().error("Failed to retrieve the struct with parameters."); + return false; + } + + // Check if the key is valid + if (!interface.isKeyValid(confKey)) { + Log::getSingleton().error("Failed to retrieve the configuration object from the singleton."); + return false; + } + + // Initialize the iDynTreeRobotState struct + const unsigned& dofs = interface.numberOfDoFs(confKey); + robotState = iDynTreeRobotState(dofs, getConfiguration().getGravityVector()); + + return true; +} + +bool WBBlock::terminate(const BlockInformation* /*blockInfo*/) +{ + return true; +} diff --git a/toolbox/src/base/WBIBlock.cpp b/toolbox/src/base/WBIBlock.cpp deleted file mode 100644 index 475ffee97..000000000 --- a/toolbox/src/base/WBIBlock.cpp +++ /dev/null @@ -1,140 +0,0 @@ -#include "WBIBlock.h" - -#include "BlockInformation.h" -#include "Error.h" -#include "WBInterface.h" -#include - -#define PARAM_IDX_1 1 // robot name -#define PARAM_IDX_2 2 // local (module) name -#define PARAM_IDX_3 3 // wbi config file -#define PARAM_IDX_4 4 // wbi list - -wbt::WBIBlock::WBIBlock() -: m_wbiConfigurationFileName("") -, m_wbiListName("") {} - -wbt::WBIBlock::~WBIBlock() { } - -unsigned wbt::WBIBlock::numberOfParameters() { return 4; } - -bool wbt::WBIBlock::configureWBIParameters(BlockInformation *blockInfo, wbt::Error *error) -{ - //parameters needed by this block: - // - YARP_ROBOT_NAME: needed by resource finder for resource lookup (for now it is taken by the environment) - // - robot: robot port. If defined overrides the one specified by wbi file - // - moduleName: local (opened) ports. - // - wbi config file name (default: yarpWholeBodyInterface.ini): specifies the wbi config file - // - wbi list (default ROBOT_TORQUE_CONTROL_JOINTS): specifies the WBI list . - // It it is a normal string, it is interpreted as a named list of joints that is loaded from the - // yarpWholeBodyInterface.ini file. If instead it s - - - //robot name - std::string robotName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_1, robotName)) { - if (error) error->message = "Cannot retrieve string from robot parameter"; - return false; - } - - //local name - std::string localName; - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_2, localName)) { - if (error) error->message = "Cannot retrieve string from localName parameter"; - return false; - } - //default local name - if (localName.empty()) localName = "WBIT"; - - //wbi config file - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_3, m_wbiConfigurationFileName)) { - if (error) error->message = "Cannot retrieve string from WBI config file parameter"; - return false; - } - - //default config file: - if (m_wbiConfigurationFileName.empty()) m_wbiConfigurationFileName = "yarpWholeBodyInterface.ini"; - - //wbi list name - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_4, m_wbiListName)) { - if (error) error->message = "Cannot retrieve string from WBI list parameter"; - return false; - } - - //default list: - if (m_wbiListName.empty()) m_wbiListName = "ROBOT_TORQUE_CONTROL_JOINTS"; - - WBInterface &interface = WBInterface::sharedInstance(); - - Error interfaceError; - if (!interface.configure(robotName, localName, m_wbiConfigurationFileName, m_wbiListName, &interfaceError)) { - if (error) error->message = "Failed to configure WholeBodyInterface with error " + interfaceError.message; - return false; - } - return true; -} - -bool wbt::WBIBlock::configureSizeAndPorts(BlockInformation *blockInfo, wbt::Error *error) -{ - //wbi config file - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_3, m_wbiConfigurationFileName)) { - if (error) error->message = "Could not read WBI configuration file parameter"; - return false; - } - - //default config file: - if (m_wbiConfigurationFileName.empty()) m_wbiConfigurationFileName = "yarpWholeBodyInterface.ini"; - - //wbi list name - if (!blockInfo->getStringParameterAtIndex(PARAM_IDX_4, m_wbiListName)) { - if (error) error->message = "Could not read WBI list parameter"; - return false; - } - - //default list: - if (m_wbiListName.empty()) m_wbiListName = "ROBOT_TORQUE_CONTROL_JOINTS"; - - WBInterface &interface = WBInterface::sharedInstance(); - - int dofs = interface.dofsForConfigurationFileAndList(m_wbiConfigurationFileName, m_wbiListName); - if (dofs == -1) { - if (error) error->message = "Failed to configure WholeBodyInterface. Could not load WBI properties from file"; - return false; - } else if (dofs == -2) { - if (error) error->message = "Failed to configure WholeBodyInterface. WBI joint list not found or failed to configure. Check if joint list exists."; - return false; - } - - return true; -} - -bool wbt::WBIBlock::initialize(BlockInformation *blockInfo, wbt::Error *error) -{ - using namespace yarp::os; - Network::init(); - if (!Network::initialized() || !Network::checkNetwork(5.0)) { - if (error) error->message = "YARP server wasn't found active"; - return false; - } - - if (!configureWBIParameters(blockInfo, error)) { - return false; - } - - WBInterface &interface = WBInterface::sharedInstance(); - if (!interface.initialize()) { - if (error) error->message = "Failed to initialize WBI"; - return false; - } - return true; -} - -bool wbt::WBIBlock::terminate(BlockInformation */*S*/, wbt::Error *error) -{ - if (!WBInterface::sharedInstance().terminate()) { - if (error) error->message = "Failed to terminate WBI"; - return false; - } - yarp::os::Network::fini(); - return true; -} diff --git a/toolbox/src/base/WBIModelBlock.cpp b/toolbox/src/base/WBIModelBlock.cpp deleted file mode 100644 index 3921b3cf2..000000000 --- a/toolbox/src/base/WBIModelBlock.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "WBIModelBlock.h" - -#include "WBInterface.h" -#include "Error.h" - -wbt::WBIModelBlock::~WBIModelBlock() {} - -bool wbt::WBIModelBlock::initialize(wbt::BlockInformation *blockInfo, wbt::Error *error) -{ - if (!configureWBIParameters(blockInfo, error)) - return false; - - if (!WBInterface::sharedInstance().initializeModel()) { - if (error) error->message = "Failed to initialize WBI"; - return false; - } - return true; -} - -bool wbt::WBIModelBlock::terminate(wbt::BlockInformation *blockInfo, wbt::Error *error) -{ - if (!WBInterface::sharedInstance().terminateModel()) { - if (error) error->message = "Failed to terminate WBI"; - return false; - } - return true; -} diff --git a/toolbox/src/base/WBInterface.cpp b/toolbox/src/base/WBInterface.cpp deleted file mode 100644 index befd4051d..000000000 --- a/toolbox/src/base/WBInterface.cpp +++ /dev/null @@ -1,318 +0,0 @@ -#include "WBInterface.h" - -#include "Error.h" -#include -#include - -namespace wbt { - - unsigned WBInterface::s_referenceCount = 0; - unsigned WBInterface::s_modelReferenceCount = 0; - - WBInterface::WBInterface() - : m_interface(0) - , m_model(0) - , m_robotList(0) - , m_configuration(0) - , m_initialized(false) - , m_modelInitialized(false) - , m_configured(false) - , m_dofs(-1) {} - - WBInterface::WBInterface(const WBInterface&) {} - WBInterface::~WBInterface() {} - WBInterface& WBInterface::operator=(const wbt::WBInterface &) { return *this; } - - WBInterface& WBInterface::sharedInstance() - { - static WBInterface instance; - return instance; - //TODO: check if the destructor get called at simulink exit - } - - wbi::wholeBodyInterface * const WBInterface::interface() { return m_interface; } - - wbi::iWholeBodyModel * const WBInterface::model() - { - if (!m_model) - return ((yarpWbi::yarpWholeBodyInterface*)m_interface)->wholeBodyModel(); - return m_model; - } - - const yarp::os::Property * const WBInterface::currentConfiguration() const - { - return m_configuration; - } - - - int WBInterface::numberOfDoFs() const { return m_dofs; } - - bool WBInterface::wbdIDListFromConfigPropAndList(const yarp::os::Property& wbiConfigProp, - const std::string& list, wbi::IDList& idList) { - // There are two ways of specifying the list throuth the mask parameter: - // either the specified parameter is a list (in the sense of YARP configuration file list, - // so something like (joint1,joint2,joint3) ) and it that case it is - // considered to by directly the list of joints to load, or otherwise - // the wbi list is just a string, and it is considered the name of the list - // in the yarpWholeBodyInterface.ini file - - // Reset the idList - idList.removeAllIDs(); - - // Check if the list string is actually a list - yarp::os::Value listAsValue; - listAsValue.fromString(list.c_str()); - - if (listAsValue.isList()) { - // If the list param is a (YARP) list, load the IDList from it - for (int jnt = 0; jnt < listAsValue.asList()->size(); jnt++) - { - yarp::os::ConstString jntName = listAsValue.asList()->get(jnt).asString(); - idList.addID(wbi::ID(jntName.c_str())); - } - } else { - // Otherwise consider the list to be a - if (!yarpWbi::loadIdListFromConfig(list, wbiConfigProp, idList)) { - return false; - } - } - return true; - } - - int WBInterface::dofsForConfigurationFileAndList(const std::string & wbiConfigFile, const std::string & list) - { - using namespace yarp::os; - - //Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 - Network network; - - ResourceFinder &resourceFinder = ResourceFinder::getResourceFinderSingleton(); - resourceFinder.configure(0, 0); - - Property configurations; - //loading defaults from configuration file - if (!configurations.fromConfigFile(resourceFinder.findFile(wbiConfigFile))) { - return -1; - } - - wbi::IDList jointList; - //parse the file to get the joint list - if (!WBInterface::wbdIDListFromConfigPropAndList(configurations,list,jointList)) { - return -2; - } - - m_dofs = jointList.size(); - - return m_dofs; - } - - bool WBInterface::configure(const std::string &robotName, - const std::string & localName, - const std::string & wbiConfigFile, - const std::string & list, - wbt::Error *error) - { - if (m_configured) return true; - - using namespace yarp::os; - - //Workaround for the fact that ResourceFinder initializes the network by itself. See YARP#1014 - Network network; - ResourceFinder &resourceFinder = ResourceFinder::getResourceFinderSingleton(); - resourceFinder.configure(0, 0); - - //parameters needed by this block: - // - YARP_ROBOT_NAME: needed by resource finder for resource lookup (for now it is taken by the environment) - // - robot: robot port. If defined overrides the one specified by wbi file - // - moduleName: local (opened) ports. - // - wbi config file name (default: yarpWholeBodyInterface.ini): specifies the wbi config file - // - wbi list (default ROBOT_TORQUE_CONTROL_JOINTS): specifies the WBI list. - // If it is a list [of style: (value1 value2 value3)] it specifies directly the list of joints to control, - // otherwise its value specifies the name of list present the wbi config file. - - if (m_configuration) { - delete m_configuration; - m_configuration = 0; - } - m_configuration = new Property(); - //loading defaults - m_wbiFilePath = resourceFinder.findFile(wbiConfigFile); - if (!m_configuration->fromConfigFile(m_wbiFilePath)) { - if (error) error->message = "Could not load the WBI configuration file"; - return false; - } - - //overwriting values - if (!robotName.empty()) - m_configuration->put("robot", robotName); - - m_configuration->put("localName", localName); - - //loading joint list - if (m_robotList) { - delete m_robotList; - m_robotList = 0; - } - - m_robotList = new wbi::IDList(); - - if (!WBInterface::wbdIDListFromConfigPropAndList(*m_configuration,list,*m_robotList)) { - if(error) error->message = "Could not load the specified WBI list (list param: " + list + " )"; - Network::fini(); - return false; - } - m_wbiListName = list; - - m_dofs = m_robotList->size(); - - m_configured = true; - - return true; - } - - void WBInterface::clearConfiguration() - { - m_configured = false; - if (m_robotList) { - delete m_robotList; - m_robotList = 0; - } - if (m_configuration) { - delete m_configuration; - m_configuration = 0; - } - } - - bool WBInterface::initialize() - { - if (m_initialized) { - s_referenceCount++; - return true; - } - - if (!m_configuration || !m_robotList) return false; - - if (m_interface) { - m_interface->close(); - delete m_interface; - m_interface = 0; - } - - m_interface = new yarpWbi::yarpWholeBodyInterface(m_configuration->find("localName").toString().c_str(), *m_configuration); - if (!m_interface) return false; - - s_referenceCount = 1; - - if (!m_interface->addJoints(*m_robotList)) { - terminate(); - return false; - } - - if (!m_interface->init()) { - terminate(); - return false; - } - - m_initialized = true; - return true; - } - - bool WBInterface::initializeModel() - { - if (m_modelInitialized) { - s_modelReferenceCount++; - return true; - } - - if (!m_configuration || !m_robotList) return false; - - if (m_model) { - m_model->close(); - delete m_model; - m_model = 0; - } - - m_model = new yarpWbi::yarpWholeBodyModel(m_configuration->find("localName").toString().c_str(), *m_configuration); - if (!m_model) return false; - - s_modelReferenceCount = 1; - - if (!m_model->addJoints(*m_robotList)) { - terminateModel(); - return false; - } - - if (!m_model->init()) { - terminateModel(); - return false; - } - - m_modelInitialized = true; - return true; - } - - bool WBInterface::terminate() - { - if (s_referenceCount > 1) { - s_referenceCount--; - return true; - } - - bool result = true; - if (m_interface) { - result = m_interface->close(); - if (result) { - delete m_interface; - m_interface = 0; - } - } - - //clean also configuration if also model has been removed - if (!m_modelInitialized) clearConfiguration(); - m_initialized = false; - s_referenceCount = 0; - return result; - } - - bool WBInterface::terminateModel() - { - if (s_modelReferenceCount > 1) { - s_modelReferenceCount--; - return true; - } - - bool result = true; - if (m_model) { - result = m_model->close(); - if (result) { - delete m_model; - m_model = 0; - } - } - - //clean also configuration if also interface has been removed - if (!m_initialized) clearConfiguration(); - m_modelInitialized = false; - s_modelReferenceCount = 0; - return result; - } - - bool WBInterface::isInterfaceInitialized() const { return m_initialized; } - - const wbi::IDList* WBInterface::wbiList() const - { - if (m_robotList) - return m_robotList; - return NULL; - } - - const std::string& WBInterface::wbiFilePath() const - { - return m_wbiFilePath; - } - - const std::string& WBInterface::wbiListName() const - { - return m_wbiListName; - } -} diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index 9ff5aac03..ae829c685 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -1,59 +1,58 @@ #include "toolbox.h" -wbt::Block* wbt::Block::instantiateBlockWithClassName(std::string blockClassName) +using namespace wbt; + +Block* Block::instantiateBlockWithClassName(std::string blockClassName) { - wbt::Block *block = NULL; - - if (blockClassName == wbt::YarpRead::ClassName) { - block = new wbt::YarpRead(); - } else if (blockClassName == wbt::YarpWrite::ClassName) { - block = new wbt::YarpWrite(); - } else if (blockClassName == wbt::YARPWBIConverter::ClassName) { - block = new wbt::YARPWBIConverter(); - } else if (blockClassName == wbt::YarpClock::ClassName) { - block = new wbt::YarpClock(); - } else if (blockClassName == wbt::MassMatrix::ClassName) { - block = new wbt::MassMatrix(); - } else if (blockClassName == wbt::ForwardKinematics::ClassName) { - block = new wbt::ForwardKinematics(); - } else if (blockClassName == wbt::SetReferences::ClassName) { - block = new wbt::SetReferences(); - } else if (blockClassName == wbt::RealTimeSynchronizer::ClassName) { - block = new wbt::RealTimeSynchronizer(); - } else if (blockClassName == wbt::SimulatorSynchronizer::ClassName) { - block = new wbt::SimulatorSynchronizer(); - } else if (blockClassName == wbt::Jacobian::ClassName) { - block = new wbt::Jacobian(); - } else if (blockClassName == wbt::GetEstimate::ClassName) { - block = new wbt::GetEstimate(); - } else if (blockClassName == wbt::InverseDynamics::ClassName) { - block = new wbt::InverseDynamics(); - } else if (blockClassName == wbt::DotJNu::ClassName) { - block = new wbt::DotJNu(); - } else if (blockClassName == wbt::GetLimits::ClassName) { - block = new wbt::GetLimits(); - } else if (blockClassName == wbt::CentroidalMomentum::ClassName) { - block = new wbt::CentroidalMomentum(); + Block* block = nullptr; + + if (blockClassName == YarpRead::ClassName) { + block = new YarpRead(); + } else if (blockClassName == YarpWrite::ClassName) { + block = new YarpWrite(); + } else if (blockClassName == ModelPartitioner::ClassName) { + block = new ModelPartitioner(); + } else if (blockClassName == YarpClock::ClassName) { + block = new YarpClock(); + } else if (blockClassName == MassMatrix::ClassName) { + block = new MassMatrix(); + } else if (blockClassName == ForwardKinematics::ClassName) { + block = new ForwardKinematics(); + } else if (blockClassName == SetReferences::ClassName) { + block = new SetReferences(); + } else if (blockClassName == RealTimeSynchronizer::ClassName) { + block = new RealTimeSynchronizer(); + } else if (blockClassName == SimulatorSynchronizer::ClassName) { + block = new SimulatorSynchronizer(); + } else if (blockClassName == Jacobian::ClassName) { + block = new Jacobian(); + } else if (blockClassName == GetMeasurement::ClassName) { + block = new GetMeasurement(); + } else if (blockClassName == InverseDynamics::ClassName) { + block = new InverseDynamics(); + } else if (blockClassName == DotJNu::ClassName) { + block = new DotJNu(); + } else if (blockClassName == GetLimits::ClassName) { + block = new GetLimits(); + } else if (blockClassName == CentroidalMomentum::ClassName) { + block = new CentroidalMomentum(); + } else if (blockClassName == SetLowLevelPID::ClassName) { + block = new SetLowLevelPID(); } #ifdef WBT_USES_ICUB else if (blockClassName == wbt::MinimumJerkTrajectoryGenerator::ClassName) { block = new wbt::MinimumJerkTrajectoryGenerator(); - } -#endif -#ifdef WBT_USES_IPOPT - else if (blockClassName == wbt::InverseKinematics::ClassName) { - block = new wbt::InverseKinematics(); + } else if (blockClassName == wbt::DiscreteFilter::ClassName) { + block = new wbt::DiscreteFilter(); } #endif - else if (blockClassName == wbt::RemoteInverseKinematics::ClassName) { - block = new wbt::RemoteInverseKinematics(); - } -#ifdef WBT_USES_CODYCO_COMMONS - else if (blockClassName == wbt::SetLowLevelPID::ClassName) { - block = new wbt::SetLowLevelPID(); - } +#ifdef WBT_USES_IPOPT + // else if (blockClassName == InverseKinematics::ClassName) { + // block = new InverseKinematics(); + // } #endif - - + // else if (blockClassName == RemoteInverseKinematics::ClassName) { + // block = new RemoteInverseKinematics(); + // } return block; } diff --git a/toolbox/src/base/toolbox.cpp b/toolbox/src/base/toolbox.cpp index 5162f3505..74468baa9 100644 --- a/toolbox/src/base/toolbox.cpp +++ b/toolbox/src/base/toolbox.cpp @@ -26,15 +26,78 @@ #include "toolbox.h" #include "Block.h" +#include "Log.h" #include "SimulinkBlockInformation.h" #include #include +static void catchLogMessages(bool status, SimStruct* S, std::string prefix) +{ + // Initialize static buffers + const unsigned bufferLen = 1024; + static char errorBuffer[bufferLen]; + static char warningBuffer[bufferLen]; + + // Notify warnings + if (!wbt::Log::getSingleton().getWarnings().empty()) { + // Get the singleton + wbt::Log& log = wbt::Log::getSingleton(); + + // Handle the prefix + std::string warningMsg; + if (!prefix.empty()) { + log.setPrefix(prefix); + warningMsg = log.getWarnings(); + log.resetPrefix(); + } + else { + warningMsg = log.getWarnings(); + } + + // Trim the message if needed + if (warningMsg.length() >= bufferLen) { + warningMsg = warningMsg.substr(0, bufferLen-1); + } + + // Forward to Simulink + sprintf(warningBuffer, "%s", warningMsg.c_str()); + ssWarning(S, warningBuffer); + log.clearWarnings(); + } + + // Notify errors + if (!status) { + // Get the singleton + wbt::Log& log = wbt::Log::getSingleton(); + + // Handle the prefix + std::string errorMsg; + if (!prefix.empty()) { + log.setPrefix(prefix); + errorMsg = log.getErrors(); + log.resetPrefix(); + } + else { + errorMsg = log.getErrors(); + } + + // Trim the message if needed + if (errorMsg.length() >= bufferLen) { + errorMsg = errorMsg.substr(0, bufferLen-1); + } + + // Forward to Simulink + sprintf(errorBuffer, "%s", errorMsg.c_str()); + ssSetErrorStatus(S, errorBuffer); + return; + } +} + // Function: MDL_CHECK_PARAMETERS #define MDL_CHECK_PARAMETERS #if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) -static void mdlCheckParameters(SimStruct *S) +static void mdlCheckParameters(SimStruct* S) { UNUSED_ARG(S); //TODO: still to find a way to call Block implementation @@ -42,8 +105,8 @@ static void mdlCheckParameters(SimStruct *S) #endif /*MDL_CHECK_PARAMETERS*/ #define MDL_SET_INPUT_PORT_DIMENSION_INFO -static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, - const DimsInfo_T *dimsInfo) +static void mdlSetInputPortDimensionInfo(SimStruct* S, int_T port, + const DimsInfo_T* dimsInfo) { //TODO: for now accept the proposed size. //If we want to change the behaviour we have to implement some callbacks @@ -51,8 +114,8 @@ static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, } #define MDL_SET_OUTPUT_PORT_DIMENSION_INFO -static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, - const DimsInfo_T *dimsInfo) +static void mdlSetOutputPortDimensionInfo(SimStruct* S, int_T port, + const DimsInfo_T* dimsInfo) { //TODO: for now accept the proposed size. //If we want to change the behaviour we have to implement some callbacks @@ -63,57 +126,64 @@ static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, // Abstract: // The sizes information is used by Simulink to determine the S-function // block's characteristics (number of inputs, s, states, etc.). -static void mdlInitializeSizes(SimStruct *S) +static void mdlInitializeSizes(SimStruct* S) { - static char errorBuffer[512]; + // Initialize the Log singleton + wbt::Log::getSingleton().clear(); + if (ssGetSFcnParamsCount(S) < 1) { - sprintf(errorBuffer, "%s", "The block type parameter must be specified"); - ssSetErrorStatus(S, errorBuffer); + wbt::Log::getSingleton().error("The block type parameter must be specified"); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); return; } - char *classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); + char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); std::string className(classNameStr); mxFree(classNameStr); - wbt::Block *block = wbt::Block::instantiateBlockWithClassName(className); + wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); //We cannot save data in PWork during the initializeSizes phase ssSetNumPWork(S, 1); - + + // Notify errors if (!block) { - sprintf(errorBuffer, "Could not create an object of type %s", className.c_str()); - ssSetErrorStatus(S, errorBuffer); + wbt::Log::getSingleton().error("Could not create an object of type " + className); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); return; } - + ssSetNumSFcnParams(S, 1 + block->numberOfParameters()); ssSetSFcnParamTunable(S, 0, false); for (unsigned i = 1; i < ssGetNumSFcnParams(S); ++i) { bool tunable = false; block->parameterAtIndexIsTunable(i - 1, tunable); ssSetSFcnParamTunable(S, i, tunable); - } - - + + #if defined(MATLAB_MEX_FILE) - if(ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)){ + if(ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { mdlCheckParameters(S); - if(ssGetErrorStatus(S)){ + if(ssGetErrorStatus(S)) { return; } - } else{ - sprintf(errorBuffer, "%s", "Number of parameters different from those defined"); - ssSetErrorStatus(S, errorBuffer); + } else { + wbt::Log::getSingleton().error("Number of parameters different from those defined"); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); return; } #endif - wbt::Error error; wbt::SimulinkBlockInformation blockInfo(S); - if (!block->configureSizeAndPorts(&blockInfo, &error)) { - sprintf(errorBuffer, "%s", error.message.substr(0, 511).c_str()); - ssSetErrorStatus(S, errorBuffer); - return; + bool ok = block->configureSizeAndPorts(&blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + + for (auto i = 0; i < ssGetNumInputPorts(S); ++i) { + // Set explicitly the inputs port to be SS_NOT_REUSABLE_AND_GLOBAL (which actually + // is already the default value). Since the toolbox supports contiguous input signals, + // this option should not be changed. + ssSetInputPortOptimOpts(S, i, SS_NOT_REUSABLE_AND_GLOBAL); + // Set input signals to be allocated in a contiguous memory storage + ssSetInputPortRequiredContiguous(S, i, true); } ssSetNumSampleTimes(S, 1); @@ -134,17 +204,15 @@ static void mdlInitializeSizes(SimStruct *S) std::vector additionalOptions = block->additionalBlockOptions(); - for (std::vector::const_iterator it = additionalOptions.begin(); - it < additionalOptions.end(); ++it) { - wbt::Data option; - if (blockInfo.optionFromKey(*it, option)) { - options |= option.uint32Data(); + for (auto additionalOption: additionalOptions) { + double option; + if (blockInfo.optionFromKey(additionalOption, option)) { + options |= static_cast(option); } } ssSetOptions(S, options); delete block; - } // Function: mdlInitializeSampleTimes ========================================= @@ -152,7 +220,7 @@ static void mdlInitializeSizes(SimStruct *S) // This function is used to specify the sample time(s) for your // S-function. You must register the same number of sample times as // specified in ssSetNumSampleTimes. -static void mdlInitializeSampleTimes(SimStruct *S) +static void mdlInitializeSampleTimes(SimStruct* S) { ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME); ssSetOffsetTime(S, 0, 0.0); @@ -165,40 +233,37 @@ static void mdlInitializeSampleTimes(SimStruct *S) // have states that should be initialized once, this is the place // to do it. #define MDL_START -static void mdlStart(SimStruct *S) +static void mdlStart(SimStruct* S) { - char *classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); + char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); std::string className(classNameStr); mxFree(classNameStr); - wbt::Block *block = wbt::Block::instantiateBlockWithClassName(className); + wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); ssSetPWorkValue(S, 0, block); - wbt::Error error; wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->initialize(&blockInfo, &error)) { - yError() << "[mdlStart]" << error.message; - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlStart]%s", error.message.substr(0, 1023 - strlen("[mdlStart]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->initialize(&blockInfo); } - + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } #define MDL_UPDATE #if defined(MDL_UPDATE) && defined(MATLAB_MEX_FILE) -static void mdlUpdate(SimStruct *S, int_T tid) +static void mdlUpdate(SimStruct* S, int_T tid) { UNUSED_ARG(tid); if (ssGetNumPWork(S) > 0) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); + wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->updateDiscreteState(&blockInfo, &error)) { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlOutputs]%s", error.message.substr(0, 1023 - strlen("[mdlOutputs]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->updateDiscreteState(&blockInfo); } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } } #endif @@ -206,17 +271,17 @@ static void mdlUpdate(SimStruct *S, int_T tid) //Initialize the state vectors of this C MEX S-function #define MDL_INITIALIZE_CONDITIONS #if defined(MDL_INITIALIZE_CONDITIONS) && defined(MATLAB_MEX_FILE) -static void mdlInitializeConditions(SimStruct *S) +static void mdlInitializeConditions(SimStruct* S) { if (ssGetNumPWork(S) > 0) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); + wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->initializeInitialConditions(&blockInfo, &error)) { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlInitializeConditions]%s", error.message.substr(0, 1023 - strlen("[mdlInitializeConditions]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->initializeInitialConditions(&blockInfo); } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } } #endif @@ -224,7 +289,7 @@ static void mdlInitializeConditions(SimStruct *S) #define MDL_DERIVATIVES #if defined(MDL_DERIVATIVES) && defined(MATLAB_MEX_FILE) -static void mdlDerivatives(SimStruct *S) +static void mdlDerivatives(SimStruct* S) { /* Add mdlDerivatives code here */ } @@ -235,40 +300,38 @@ static void mdlDerivatives(SimStruct *S) // Abstract: // In this function, you compute the outputs of your S-function // block. -static void mdlOutputs(SimStruct *S, int_T tid) +static void mdlOutputs(SimStruct* S, int_T tid) { UNUSED_ARG(tid); if (ssGetNumPWork(S) > 0) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); + wbt::SimulinkBlockInformation blockInfo(S); - if (!block || !block->output(&blockInfo, &error)) { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlOutputs]%s", error.message.substr(0, 1023 - strlen("[mdlOutputs]")).c_str()); - ssSetErrorStatus(S, errorBuffer); + bool ok = false; + if (block) { + ok = block->output(&blockInfo); } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); } - } -static void mdlTerminate(SimStruct *S) +static void mdlTerminate(SimStruct* S) { if (ssGetNumPWork(S) > 0 && ssGetPWork(S)) { - wbt::Block *block = static_cast(ssGetPWorkValue(S, 0)); - wbt::Error error; + wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); + wbt::SimulinkBlockInformation blockInfo(S); + bool ok = false; if (block) { - if (block->terminate(&blockInfo, &error)) { - delete block; - ssSetPWorkValue(S, 0, NULL); - } else { - static char errorBuffer[1024]; - sprintf(errorBuffer, "[mdlTerminate]%s", error.message.substr(0, 1023 - strlen("[mdlTerminate]")).c_str()); - ssSetErrorStatus(S, errorBuffer); - } + ok = block->terminate(&blockInfo); } - } + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + if (ok) { + delete block; + ssSetPWorkValue(S, 0, NULL); + } + } } // Required S-function trailer