diff --git a/.ci/install.sh b/.ci/install.sh index 4aa0efec3..2ea28f664 100755 --- a/.ci/install.sh +++ b/.ci/install.sh @@ -16,9 +16,7 @@ cmake --build . --target install # Install Yarp cd $HOME/git -#git clone --depth 1 -b $DEPS_BRANCH https://github.com/robotology/yarp.git -echo "Checking out devel. Use master as soon as it will be fixed." -git clone --depth 1 -b devel https://github.com/robotology/yarp.git +git clone --depth 1 -b $DEPS_BRANCH https://github.com/robotology/yarp.git cd yarp mkdir build && cd build cmake .. \ @@ -30,9 +28,7 @@ cmake --build . --config $TRAVIS_BUILD_TYPE --target install # Install icub-main cd $HOME/git -echo "Checking out devel. Use master as soon as it will be fixed." -git clone --depth 1 -b devel https://github.com/robotology/icub-main.git -# git clone --depth 1 -b $DEPS_BRANCH https://github.com/robotology/icub-main.git + git clone --depth 1 -b $DEPS_BRANCH https://github.com/robotology/icub-main.git cd icub-main mkdir build && cd build cmake .. \ @@ -51,3 +47,14 @@ cmake .. \ -DCMAKE_BUILD_TYPE=$TRAVIS_BUILD_TYPE \ -DCMAKE_INSTALL_PREFIX=$DEPS_CACHE cmake --build . --config $TRAVIS_BUILD_TYPE --target install $CMAKE_BUILD_OPTIONS + +# Install BlockFactory +cd $HOME/git +git clone --depth 1 https://github.com/robotology/blockfactory +cd blockfactory +mkdir build && cd build +cmake .. \ + -G"$TRAVIS_CMAKE_GENERATOR" \ + -DCMAKE_BUILD_TYPE=$TRAVIS_BUILD_TYPE \ + -DCMAKE_INSTALL_PREFIX=$DEPS_CACHE +cmake --build . --config $TRAVIS_BUILD_TYPE --target install $CMAKE_BUILD_OPTIONS diff --git a/.github/CONTRIBUTING.md b/.github/CONTRIBUTING.md index 6c2bf8c93..2275ab41c 100644 --- a/.github/CONTRIBUTING.md +++ b/.github/CONTRIBUTING.md @@ -2,9 +2,9 @@ First off, thanks for your interest in contributing to this project! :tada: -The following is a set of guidelines for contributing to WB-Toolbox. This project is developed at the Italian Institute of Technology and it is part of our [robotology](https://github.com/robotology) organization. +The following is a set of guidelines for contributing to Whole-Body Toolbox. This project is developed at the Italian Institute of Technology and it is part of our [robotology](https://github.com/robotology) organization. -Keep in mind that the following are mostly guidelines, not strict rules. Use your best judgement and feel free to propose changes to this document in a pull request. This project is not yet mature as other ones inside our organization, we'd be glad to hear your voice. +Keep in mind that the following are mostly guidelines, not strict rules. Use your best judgement and feel free to propose changes to this document in a pull request. #### Table of contents @@ -14,6 +14,7 @@ Keep in mind that the following are mostly guidelines, not strict rules. Use you - [CMake](#cmake) - [Git](#git) - [Documentation](#documentation) +- [Website](#website) - [Policies](#policies) - [Versioning](#versioning) - [Deprecations](#deprecations) @@ -44,7 +45,7 @@ Furthermore: - Add an empty line between project, system headers, and `std` headers. - Forward-declare classes into namespaces. - Do not use `#pragma once`, use instead `#ifndef ... #define ... #endif` macro. -- The defines macros should have the `WBT__H` format. +- The defines macros should have the `NS1_NS2__H` format. - The final `#endif` should have a comment containing the closed `#ifndef`. - When possible, use explicit declaration and explicit instantiation of templates. - When not possible, use [three-files `-inl.h`](http://drake.mit.edu/cxx_inl.html) pattern approach. @@ -52,8 +53,8 @@ Furthermore: Example: ```cpp -#ifndef WBT_JACOBIAN_H -#define WBT_JACOBIAN_H +#ifndef WBT_BLOCK_JACOBIAN_H +#define WBT_BLOCK_JACOBIAN_H #include "Jacobian.h" #include "RobotInterface.h" @@ -66,14 +67,16 @@ Example: #include namespace wbt { - class Jacobian; + namespace block { + class Jacobian; + } // namespace block } // namespace wbt -class wbt::Jacobian +class wbt::block::Jacobian { ... -#endif // WBT_JACOBIAN_H +#endif // WBT_BLOCK_JACOBIAN_H ``` ### CMake @@ -96,34 +99,56 @@ class wbt::Jacobian ## Documentation -- All the new classes and methods of your PR should contain the doxygen documentation in their headers. For checking its render before pushing, a possible way is the following: - ```bash - # From the build directory: +- All the new classes and methods of your PR should contain the doxygen documentation in their headers. For checking its render before pushing, proceed as follows: + ``` + mkdir buildDox + cd buildDox + cmake -DWBT_BUILD_DOCS=ON .. make dox + # If there are any errors, fix them. cd doc/doxygen/html - python -m http.server + python3 -m http.server # Browse to http://localhost:8000 ``` -- Your PR should add a changelog in the [doc/release](/doc/release) file of the upcoming version. +- Your PR should add a changelog in the [doc/release](/doc/release) file of the [upcoming version](#versioning) (minor release if the PR targets `master`, major release if it targets `devel`). + +## Website + +Our website is based on the [mkdocs](https://github.com/mkdocs/mkdocs) framework and the [mkdocs-material](https://github.com/squidfunk/mkdocs-material) theme. + +The easier way to modify a page of website is doing it directly from the website. In the top-right corner of every page, on the same line of the page title, you will find an pencil icon. If you click on the icon, the markdown file that creates the webpage opens in the GitHub web editor. Here you can modify the document and create a pull request. + +If the edits you want to submit are not trivial, or if you want to check how the website will be rendered, you need to clone the repository and modify with your favourite text editor the files inside the [`doc/mkdocs/data`](/doc/mkdocs/data) folder. Afterwards, you can generate and browse a local website executing the following commands: + +``` +mkdir buildWebsite +cd buildWebsite +cmake -DWBT_BUILD_DOCS=ON .. +make mkdocs +# If there are any errors, fix them. +cd doc/mkdocs +python3 -m http.server +# Browse to http://localhost:8000 +``` ## Policies ## Versioning -After an assessment period, with few releases without any defined versioning policy, starting from the release `4` WB-Toolbox follows the following pattern: +Starting from `v5`, this project hosts only a blockfactory plugin library. All the public APIs have been moved to [robotology/blockfactory](https://github.com/robotology/blockfactory). The versioning policy is not anymore related to public APIs. -#### Major releases, e.g. `4` +The next sections use the definition of **user code**: it represents all the functionalities that can be built with the `WBT` library. It includes Simulink models, autogenerated code from Simulink models, and C++ code that directly uses `WBT` as a C++ library. -- We use a fast rollout numbering, publishing a new major release every devel freeze. This happens when we decide that the amount of new features are worth a new release. -- Major releases may or may not break APIs. Read [Deprecations](#deprecations) for further information. +#### Major releases, e.g. `v5` -#### Minor releases, e.g. `4.1` +- We use a fast rollout numbering, publishing a new major release every `devel` freeze. This happens when we decide that the amount of new features are worth a new release. +- Major releases may or may not break user code. Read [Deprecations](#deprecations) for further information. + +#### Minor releases, e.g. `v5.1` - Minor releases may contain hot-fixes applied directly to `master` (and then merged to `devel`). -- APIs stability is preserved. +- User code is preserved. ## Deprecations -The following policy is still a rule-of-thumb. We are not yet enforcing it, but soon we will. Take note of it because this will be valid as soon as we reach an acceptable API stability. - -- Functions and classes which are marked as deprecated in one major release will be kept at least for another major release. This means that a deprecated class in version `4` will still be present in version `5`, but it might be deleted in version `6`. +Functions and classes which are marked as deprecated in one major release will be kept at least for another major release. This means that a deprecated class in version `v4` will still be present in version `v5`, but it might be deleted in version `v6`. This should give users an acceptable time buffer to update their models. diff --git a/.travis.yml b/.travis.yml index 6522fa155..031c9a3ba 100644 --- a/.travis.yml +++ b/.travis.yml @@ -1,4 +1,4 @@ -dist: trusty +dist: xenial language: cpp services: docker @@ -9,10 +9,24 @@ cache: - $HOME/.ccache - $HOME/Library/Caches/Homebrew +addons: + apt: + packages: + - docker-ce + homebrew: + update: true + packages: + - ccache + - eigen + - ace + - tinyxml + - gsl + stages: - test # Default stage with job matrix - osx - - deploy + - name: deploy + if: fork = false AND branch IN (master, devel) AND type != pull_request AND env(DEPLOY_TOKEN) IS present compiler: - gcc @@ -49,33 +63,35 @@ before_script: travis_terminate 1 fi - docker pull $DOCKER_IMAGE_NAME - -script: + # Start the container in the background - >- - docker run -it \ - -v $TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR \ - -v $HOME/.ccache:$HOME/.ccache \ - -w $TRAVIS_BUILD_DIR \ - --env-file .ci/env-file \ + docker run -it -d \ + --name ci \ + -v "$TRAVIS_BUILD_DIR:$TRAVIS_BUILD_DIR" \ + -v "$HOME/.ccache:$HOME/.ccache" \ + -w "$TRAVIS_BUILD_DIR" \ + --env-file "$TRAVIS_BUILD_DIR/.ci/env-file" \ $DOCKER_IMAGE_NAME \ - sh .ci/script.sh + bash + +script: + - docker exec ci ./.ci/script.sh # ========== # STAGE: osx # ========== stage_osx: + before_install: &osx_before_install + # Setup ccache + - export PATH="/usr/local/opt/ccache/libexec:$PATH" install: &osx_install # Setup the dependencies folder - export DEPS_CACHE=$HOME/deps - export PATH=$PATH:$DEPS_CACHE/bin - export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$DEPS_CACHE/lib - export CMAKE_PREFIX_PATH=$CMAKE_PREFIX_PATH:$DEPS_CACHE - # Setup ccache - - brew install ccache - - export PATH="/usr/local/opt/ccache/libexec:$PATH" # Install dependencies - - brew install eigen ace tinyxml gsl - cd $TRAVIS_BUILD_DIR/.ci - ./install.sh || travis_terminate 1 script: &osx_script @@ -149,14 +165,16 @@ jobs: - &osx_template stage: osx os: osx - osx_image: xcode9.3 - before_install: skip + osx_image: xcode10.1 + services: + before_install: *osx_before_install install: *osx_install before_script: skip script: *osx_script after_failure: skip after_success: skip after_script: skip + compiler: env: TRAVIS_CMAKE_GENERATOR="Xcode" TRAVIS_BUILD_TYPE="Debug" @@ -172,7 +190,6 @@ jobs: stage: deploy compiler: cache: - if: fork = false AND branch = "master" AND type != pull_request before_install: skip install: *deploy_install before_script: skip @@ -194,8 +211,8 @@ jobs: - doxygen-gui - graphviz - python3-pip - - <<: *deploy_template - if: fork = false AND branch = "devel" AND type != pull_request + - python3-setuptools + - python3-wheel # notifications: # email: diff --git a/CMakeLists.txt b/CMakeLists.txt index b79a2ee04..9a58b805d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,15 +3,48 @@ # GNU Lesser General Public License v2.1 or any later version. cmake_minimum_required(VERSION 3.5) -project(WB-Toolbox LANGUAGES CXX VERSION 4) +project(WB-Toolbox LANGUAGES CXX VERSION 5) if(WBT_BUILD_DOCS) add_subdirectory(doc) return() endif() +# ================= +# FIND DEPENDENCIES +# ================= + +# Matlab is not a compile dependency. If it is found, the Simulink library +# and all the Matlab-related resources are installed. +find_package(Matlab COMPONENTS + MX_LIBRARY + ENG_LIBRARY + MAIN_PROGRAM + # SIMULINK # Requires CMake > 3.7 +) + +find_package(YCM 0.9 REQUIRED) +find_package(YARP 3.0.0 REQUIRED) + +# Fail if YARP is not compiled as shared library +# see https://github.com/robotology/codyco-modules/issues/44 +if(NOT ${YARP_IS_SHARED_LIBRARY}) + message(FATAL_ERROR "YARP was found, but it was compiled as static library. A shared library version of YARP is required.") +endif() + +find_package(iDynTree 0.7.2 REQUIRED) + +# This find loads Eigen. +# It would be nice using the EigenConfig shipped with eigen package, but +# on Ubuntu 16.04 eigen is old and provides only FindEigen. +find_package(Eigen3 REQUIRED) + +# ==================== +# OPTIONS AND INCLUDES +# ==================== + # C++ standard -set(CMAKE_CXX_STANDARD 11) +set(CMAKE_CXX_STANDARD 14) set(CMAKE_CXX_STANDARD_REQUIRED ON) # Build type @@ -31,35 +64,69 @@ else() option(WBT_SHARED_LIBRARY "Compile WBToolbox as a shared library" TRUE) endif() set(BUILD_SHARED_LIBS ${WBT_SHARED_LIBRARY}) -if(NOT BUILD_SHARED_LIBS) - set(CMAKE_POSITION_INDEPENDENT_CODE ON) -endif() +set(CMAKE_POSITION_INDEPENDENT_CODE ON) # Tweak linker flags in Linux if(UNIX AND NOT APPLE) - get_filename_component(LINKER_BIN ${CMAKE_LINKER} NAME) - if(${LINKER_BIN} STREQUAL "ld") - set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--unresolved-symbols=report-all") # TODO + if("${CMAKE_BUILD_TYPE}" STREQUAL "Debug") + get_filename_component(LINKER_BIN ${CMAKE_LINKER} NAME) + if(${LINKER_BIN} STREQUAL "ld") + set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--unresolved-symbols=report-all") + endif() + endif() +endif() + +# Settings for RPATH +if(NOT MSVC) + option(WBT_ENABLE_RPATH "Enable RPATH installation" TRUE) + mark_as_advanced(WBT_ENABLE_RPATH) +endif() + +# Export all symbols in Windows +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# Add a postfix to Windows libraries compiled in debug +if(MSVC) + set(CMAKE_DEBUG_POSTFIX "d") +endif() + +# Control where binaries and libraries are placed in the build folder. +# This simplifies tests running in Windows. +set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") +set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") +set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") + +include(AddInstallRPATHSupport) +add_install_rpath_support(BIN_DIRS ${CMAKE_INSTALL_PREFIX}/bin + ${CMAKE_INSTALL_PREFIX}/lib + DEPENDS WBT_ENABLE_RPATH + USE_LINK_PATH) + +# Get include-what-you-see information when compiling +option(WBT_USE_IWYU "Get the output of include-what-you-use" OFF) +if(WBT_USE_IWYU) + find_program(IWYU_PATH NAMES include-what-you-use iwyu) + if(IWYU_PATH) + set(CMAKE_CXX_INCLUDE_WHAT_YOU_USE ${IWYU_PATH}) endif() endif() # Add custom functions / macros list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -find_package(YCM 0.9 REQUIRED) +include(GNUInstallDirs) include(YCMDefaultDirs) - -find_package(Matlab COMPONENTS - MX_LIBRARY - ENG_LIBRARY - MAIN_PROGRAM - # SIMULINK # Requires CMake > 3.7 -) +include(InstallBasicPackageFiles) +include(AddUninstallTarget) if(NOT WBT_USES_MATLAB) - option(WBT_USES_MATLAB "Compile also Matlab-related components" ${Matlab_FOUND}) + option(WBT_USES_MATLAB "Install Matlab / Simulink resources" ${Matlab_FOUND}) endif() +# ============== +# SUBDIRECTORIES +# ============== + add_subdirectory(deps) add_subdirectory(toolbox) @@ -67,4 +134,3 @@ if(WBT_USES_MATLAB) add_subdirectory(matlab) endif() -include(AddUninstallTarget) diff --git a/README.md b/README.md index 87b258b2a..7dda9ded2 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,7 @@ -# Whole Body Toolbox +# Whole-Body Toolbox -[![Build Status (Linux/macOS) (master)](https://img.shields.io/travis/robotology/wb-toolbox/master.svg?logo=travis&label=master)](https://travis-ci.org/robotology/wb-toolbox) -[![Build Status (Linux/macOS) (devel)](https://img.shields.io/travis/robotology/wb-toolbox/devel.svg?logo=travis&label=devel)](https://travis-ci.org/robotology/wb-toolbox) -[![Build status (Windows) (master)](https://img.shields.io/appveyor/ci/robotology/wb-toolbox/master.svg?logo=appveyor&label=master)](https://ci.appveyor.com/project/robotology/wb-toolbox) -[![Build status (Windows) (devel)](https://img.shields.io/appveyor/ci/robotology/wb-toolbox/devel.svg?logo=appveyor&label=devel)](https://ci.appveyor.com/project/robotology/wb-toolbox) +[![Build Status (master)](https://img.shields.io/travis/robotology/wb-toolbox/master.svg?logo=travis&label=master)](https://travis-ci.org/robotology/wb-toolbox) +[![Build Status (devel)](https://img.shields.io/travis/robotology/wb-toolbox/devel.svg?logo=travis&label=devel)](https://travis-ci.org/robotology/wb-toolbox) [![Codacy Badge](https://api.codacy.com/project/badge/Grade/1c726331d58b4a1ebfba1c25d15f00ad)](https://www.codacy.com/app/diegoferigo/wb-toolbox?utm_source=github.com&utm_medium=referral&utm_content=robotology/wb-toolbox&utm_campaign=Badge_Grade) [![License](https://img.shields.io/badge/license-LGPL-19c2d8.svg)](https://github.com/robotology/wb-toolbox/blob/master/LICENSE.LGPL2) @@ -12,28 +10,24 @@ [![GitPitch](https://gitpitch.com/assets/badge.svg)](https://gitpitch.com/robotology/wb-toolbox/master?p=.presentations/WBToolbox2) -### A Simulink Toolbox for Whole Body Control +### A Simulink Toolbox for Whole-Body Control -This toolbox allows non-programming experts and researchers approaching _Whole Body Control_ to more easily develop controllers on either simulated or real `YARP`-based robotic platforms.
+This toolbox allows non-programming experts and researchers approaching _Whole-Body Control_ to more easily develop controllers on either simulated or real `YARP`-based robotic platforms.
Develop to deployment time is minimized by exploiting the rich variety of Simulink's toolboxes and its capabilities on rapid prototyping and visual debugging. -Visit the [WB-Toolbox Website](https://robotology.github.io/wb-toolbox/) for more informations. +`WBT` is based on the dataflow framework [`blockfactory`](https://github.com/robotology/blockfactory). Visit the [Whole-Body Toolbox Website](https://robotology.github.io/wb-toolbox/) for more information. -### +## Who uses `WBT` -![](http://drive.google.com/uc?export=view&id=0B6zDGh11iY6oc0gtM0lMdDNweWM) +`WBT` is used extensively in the controllers stored in [robotology/whole-body-controllers](https://github.com/robotology/whole-body-controllers). -## Who use `WB-Toolbox` - -`WB-Toolbox` is used extensively in the controllers stored in [robotology-playground/whole-body-controllers](https://github.com/robotology-playground/whole-body-controllers). - -This video shows the latest results on the iCub robot achieved in the EU project CoDyCo in which a top level controller implemented with the `WB-Toolbox` achieves a running `100 Hz` rate. +This video shows the latest results on the iCub robot achieved in the EU project CoDyCo in which a top level controller implemented with the `WBT` achieves a running `100 Hz` rate. [![IMAGE ALT TEXT](http://img.youtube.com/vi/VrPBSSQEr3A/0.jpg)](https://youtu.be/UXU3KSa201o "iCub balancing on one foot via external force control and interacting with humans") ### Citing this work -Please cite the following publication if you are using WB-Toolbox for your own research and/or robot controllers: +Please cite the following publication if you are using Whole-Body Toolbox for your own research and/or robot controllers: > Romano F., Traversaro S., Pucci D., Nori F.
> **A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems**
@@ -52,7 +46,7 @@ year={2017}, ### Acknowledgments -The development of WB-Toolbox is supported by: +The development of Whole-Body Toolbox is supported by: - FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics) - H2020 EU projects AnDy (No. 731540 H2020-ICT-2016-1) diff --git a/cmake/AddGeneratedCodeLib.cmake b/cmake/AddGeneratedCodeLib.cmake deleted file mode 100644 index f03ec7dd7..000000000 --- a/cmake/AddGeneratedCodeLib.cmake +++ /dev/null @@ -1,110 +0,0 @@ -include(CMakeParseArguments) - -macro(add_generated_code_lib) - -# ================================== -# PARSE AND PROCESS MACRO PARAMETERS -# ================================== - -set(_oneValueArgs MODELNAME SOURCE_FOLDER) -set(PREFIX "add_generated_code_lib") -string(TOUPPER ${PREFIX} PREFIX) - -cmake_parse_arguments(${PREFIX} - "${_options}" - "${_oneValueArgs}" - "${_multiValueArgs}" - "${ARGN}") - -# Check if the MODELNAME has been passed -set(SIMULINK_MODELNAME ${${PREFIX}_MODELNAME}) -if(NOT DEFINED ${PREFIX}_MODELNAME OR ${PREFIX}_MODELNAME STREQUAL "") - message(FATAL_ERROR "Model name not passed or empty") -endif() - -# If source folder argument was passed, assume to find here ${SIMULINK_MODELNAME}.cpp file -if(NOT DEFINED ${PREFIX}_SOURCE_FOLDER) - get_filename_component(CPP_ABSPATH ${SIMULINK_MODELNAME}.cpp ABSOLUTE) - if(NOT EXISTS ${CPP_ABSPATH}) - message(FATAL_ERROR "Failed to find ${SIMULINK_MODELNAME}.cpp. Use SOURCE_FOLDER argument \ - if it is not contained in the current directory.") - endif() - set(AUTOGEN_ABSPATH ${CMAKE_CURRENT_SOURCE_DIR}) -else() - get_filename_component(AUTOGEN_ABSPATH ${${PREFIX}_SOURCE_FOLDER} ABSOLUTE) - # Check that the directory exists - if(NOT EXISTS ${AUTOGEN_ABSPATH}) - message(FATAL_ERROR "Passed source directory ${${PREFIX}_SOURCE_FOLDER} does not exist.") - endif() - # Look for the cpp file - if(NOT EXISTS ${AUTOGEN_ABSPATH}/${SIMULINK_MODELNAME}.cpp) - message(FATAL_ERROR "Failed to find ${SIMULINK_MODELNAME}.cpp in ${${PREFIX}_SOURCE_FOLDER} directory.") - endif() -endif() - -# ===================================================== -# FIND DEPENDENCIES FOR BUILDING THE AUTOGENERATED CODE -# ===================================================== - -find_package(WBToolbox 4 REQUIRED - COMPONENTS ToolboxCoder WBToolboxLibrary) - -# ============================================== -# FIND THE defines.txt FILE AND LOAD ITS CONTENT -# ============================================== - -# The folder containing the autogenerated sources should contain a defines.txt file -get_filename_component(CODER_DEFINES_FILE ${AUTOGEN_ABSPATH}/defines.txt ABSOLUTE) -if(EXISTS ${CODER_DEFINES_FILE}) - message(STATUS "Found autogenerated sources for ${SIMULINK_MODELNAME} model.") -else() - message(FATAL_ERROR "Folder ${AUTOGEN_ABSPATH} does not contain defines.txt. It does not look \ - a valid folder containing autogenerated sources.") -endif() - -# Parse the defines exported by Simulink Coder -file(STRINGS ${CODER_DEFINES_FILE} CODER_DEFINES) - -# ============================= -# SETUP THE INCLUDE DIRECTORIES -# ============================= - -# Model includes -set(CODER_INCLUDES "${AUTOGEN_ABSPATH}") - -if(NOT Matlab_ROOT_DIR) - message(FATAL_ERROR "Variable Matlab_ROOT_DIR not found. Call find_package(Matlab) before using this macro") -endif() - -# System includes -set(CODER_INCLUDES_SYSTEM "" - "${Matlab_ROOT_DIR}/extern/include" - "${Matlab_ROOT_DIR}/simulink/include" - "${Matlab_ROOT_DIR}/rtw/c/include" - "${Matlab_ROOT_DIR}/rtw/c/src" - "${Matlab_ROOT_DIR}/rtw/c/src/ext_mode/common" -) - -# ============== -# LIBRARY TARGET -# ============== - -# Simulink Coder Headers -# Here using GLOB is necessary because Simulink generates more or less headers depending -# on the Coder configuration of the model -file(GLOB CODER_HEADERS ${AUTOGEN_ABSPATH}/*.h) - -# Simulink Coder source file -set(CODER_SOURCES "${AUTOGEN_ABSPATH}/${SIMULINK_MODELNAME}.cpp") - -# Set the target name for the autogenerated sources -set(AUTOGEN_LIB "${SIMULINK_MODELNAME}_LIB") - -add_library(${AUTOGEN_LIB} SHARED ${CODER_HEADERS} ${CODER_SOURCES}) - -target_compile_definitions(${AUTOGEN_LIB} PUBLIC ${CODER_DEFINES}) -target_include_directories(${AUTOGEN_LIB} PUBLIC ${CODER_INCLUDES}) -target_include_directories(${AUTOGEN_LIB} SYSTEM PUBLIC ${CODER_INCLUDES_SYSTEM}) -target_link_libraries(${AUTOGEN_LIB} PUBLIC WBToolbox::Coder WBToolbox::Library) - -endmacro() diff --git a/cmake/ExtraPackageConfigVars.cmake.in b/cmake/ExtraPackageConfigVars.cmake.in deleted file mode 100644 index 67cf896f7..000000000 --- a/cmake/ExtraPackageConfigVars.cmake.in +++ /dev/null @@ -1,36 +0,0 @@ -set(NAME "WBToolbox") - -foreach(comp (ToolboxCore ToolboxCoder ToolboxMex WBToolboxLibrary)) - if(${${comp}_FOUND}) - string(CONCAT FOUND_COMPONENTS_LIST ${FOUND_COMPONENTS_LIST} " ${comp}") - endif() -endforeach() - -foreach(comp ${${NAME}_FIND_COMPONENTS}) - # Match the component status with the find_dependency exit variable - set(${NAME}_${comp}_FOUND ${${comp}_FOUND}) -endforeach() - -check_required_components(${NAME}) - -if(${NAME}_FOUND) - message(STATUS "Found WBToolbox ${${NAME}_FIND_VERSION} (Components:${FOUND_COMPONENTS_LIST})") -endif() - -if(NOT ${NAME}_FIND_QUIETLY) - foreach(comp ${${NAME}_FIND_COMPONENTS}) - # If a component is not found, tell the user its name - if((NOT ${NAME}_FIND_REQUIRED_${comp}) AND (NOT ${comp}_FOUND)) - # status if it is OPTIONAL_COMPONENT - message(STATUS "WBToolbox: OPTIONAL component ${comp} NOT found") - elseif(NOT ${comp}_FOUND) - # warning if it is REQUIRED - message(WARNING "WBToolbox: REQUIRED component ${comp} NOT found") - # Note: find_package will exit with a fatal error but without telling - # what is missing - endif() - endforeach() -endif() - -unset(NAME) -unset(FOUND_COMPONENTS_LIST) diff --git a/cmake/Utilities.cmake b/cmake/Utilities.cmake deleted file mode 100644 index 85c4f5587..000000000 --- a/cmake/Utilities.cmake +++ /dev/null @@ -1,23 +0,0 @@ -include(CMakeParseArguments) - -macro(CONFIGURE_BLOCK) - - set(_options ) - set(_oneValueArgs BLOCK_NAME - LIST_PREFIX - GROUP) - set(_multiValueArgs SOURCES - HEADERS - ) - - cmake_parse_arguments(_ARS "${_options}" - "${_oneValueArgs}" - "${_multiValueArgs}" - "${ARGN}") - - set_property(GLOBAL APPEND PROPERTY ${_ARS_LIST_PREFIX}_HEADERS ${_ARS_HEADERS}) - set_property(GLOBAL APPEND PROPERTY ${_ARS_LIST_PREFIX}_SOURCES ${_ARS_SOURCES}) - - source_group("${_ARS_GROUP}\\${_ARS_BLOCK_NAME}\\Headers" FILES ${_ARS_HEADERS}) - source_group("${_ARS_GROUP}\\${_ARS_BLOCK_NAME}\\Sources" FILES ${_ARS_SOURCES}) -endmacro() \ No newline at end of file diff --git a/deps/CMakeLists.txt b/deps/CMakeLists.txt index be8f55c47..ca708c46b 100644 --- a/deps/CMakeLists.txt +++ b/deps/CMakeLists.txt @@ -2,9 +2,4 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -if(WBT_USES_MATLAB) - add_subdirectory(MxAnyType) -endif() - -add_subdirectory(sharedlibpp) add_subdirectory(thrift) diff --git a/deps/MxAnyType/CMakeLists.txt b/deps/MxAnyType/CMakeLists.txt deleted file mode 100644 index 8f8083cf5..000000000 --- a/deps/MxAnyType/CMakeLists.txt +++ /dev/null @@ -1,74 +0,0 @@ -# Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -cmake_minimum_required(VERSION 3.5) -project(MxAnyType LANGUAGES CXX VERSION 0.1) - -# Configure the project -# ===================== - -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -include(GNUInstallDirs) - -# Export all symbols in Windows -set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) - -# Add a postfix to Windows libraries compiled in debug -if(MSVC) - set(CMAKE_DEBUG_POSTFIX "d") -endif() - -# Control where binaries and libraries are placed in the build folder. -# This simplifies tests running in Windows. -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") -set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") - -# Build the library -# ================= - -# Add the target -add_library(MxAnyType - include/AnyType.h - include/MxAnyType.h - src/MxAnyType.cpp) - -set_target_properties(MxAnyType PROPERTIES - VERSION ${PROJECT_VERSION} - PUBLIC_HEADER include/AnyType.h -) - -# Find Matlab resources -find_package(Matlab REQUIRED MX_LIBRARY) -target_include_directories(MxAnyType SYSTEM PRIVATE "${Matlab_INCLUDE_DIRS}") -target_link_libraries(MxAnyType ${Matlab_MX_LIBRARY}) - -# Setup the include directories -target_include_directories(MxAnyType PUBLIC - $ - $) - -# Build tests -# =========== - -# add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/tests) - -# Install -# ======= - -install(TARGETS MxAnyType - EXPORT MxAnyTypeExport - LIBRARY DESTINATION "${CMAKE_INSTALL_LIBDIR}" - ARCHIVE DESTINATION "${CMAKE_INSTALL_LIBDIR}" - RUNTIME DESTINATION "${CMAKE_INSTALL_BINDIR}" - PUBLIC_HEADER DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/MxAnyType" -) - -include(InstallBasicPackageFiles) -install_basic_package_files(MxAnyType - VERSION ${PROJECT_VERSION} - COMPATIBILITY AnyNewerVersion - EXPORT MxAnyTypeExport - NO_CHECK_REQUIRED_COMPONENTS_MACRO) diff --git a/deps/MxAnyType/include/AnyType.h b/deps/MxAnyType/include/AnyType.h deleted file mode 100644 index 24e06e45c..000000000 --- a/deps/MxAnyType/include/AnyType.h +++ /dev/null @@ -1,68 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef ANYTYPE_H -#define ANYTYPE_H - -#include -#include -#include - -class AnyType; - -using AnyTypeSPtr = std::shared_ptr; -using AnyCell = std::vector; -using AnyStruct = std::unordered_map; - -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/deps/MxAnyType/include/MxAnyType.h b/deps/MxAnyType/include/MxAnyType.h deleted file mode 100644 index c7f5997b3..000000000 --- a/deps/MxAnyType/include/MxAnyType.h +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef MXANYTYPE_H -#define MXANYTYPE_H - -#include "AnyType.h" - -#include - -#include -#include -#include -#include - -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); - -public: - MxAnyType() = delete; - MxAnyType(const mxArray* m, bool validateId = false); - ~MxAnyType() override = 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/deps/MxAnyType/src/MxAnyType.cpp b/deps/MxAnyType/src/MxAnyType.cpp deleted file mode 100644 index 88578807b..000000000 --- a/deps/MxAnyType/src/MxAnyType.cpp +++ /dev/null @@ -1,262 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "MxAnyType.h" - -#include -#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(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) { - return false; - } - if (!fieldContent) { - 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/deps/sharedlibpp/.gitignore b/deps/sharedlibpp/.gitignore deleted file mode 100644 index 956ac116d..000000000 --- a/deps/sharedlibpp/.gitignore +++ /dev/null @@ -1,2 +0,0 @@ -build -*.*~ diff --git a/deps/sharedlibpp/AUTHORS b/deps/sharedlibpp/AUTHORS deleted file mode 100644 index f23bcb8fd..000000000 --- a/deps/sharedlibpp/AUTHORS +++ /dev/null @@ -1,10 +0,0 @@ -Author and contributor list ---------------------------- - -Ali Paikan -Daniele E. Domenichelli -Paul Fitzpatrick -Damiano Enerli -Francesco Romano -Nicolò Genesio -Silvio Traversaro diff --git a/deps/sharedlibpp/CMakeLists.txt b/deps/sharedlibpp/CMakeLists.txt deleted file mode 100644 index 114b78150..000000000 --- a/deps/sharedlibpp/CMakeLists.txt +++ /dev/null @@ -1,86 +0,0 @@ -# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) -# All rights reserved. -# -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. See the accompanying LICENSE file for details. - -cmake_minimum_required(VERSION 3.5) - -project(shlibpp - VERSION 0.0.1 - LANGUAGES CXX) -set(PROJECT_DESCRIPTION "Tiny cross-platform plug-in system (dll, so, dylib)") - -# For now keep YCM as a soft dependency -find_package(YCM 0.8.1 QUIET) -if(NOT YCM_FOUND) - list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake") -endif() - -# Set output directories for targets -include(GNUInstallDirs) -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") -set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") - -# Set postfix for debug builds on MSVC -if(MSVC) - set(CMAKE_DEBUG_POSTFIX "d") -endif() - -# Encourage user to specify a build type (e.g. Release, Debug, etc.), otherwise set it to Release. -if(NOT CMAKE_CONFIGURATION_TYPES) - if(NOT CMAKE_BUILD_TYPE) - message(STATUS "Setting build type to 'Release' as none was specified.") - set_property(CACHE CMAKE_BUILD_TYPE PROPERTY VALUE "Release") - endif() -endif() - -# Shared/Dynamic or Static library? -option(BUILD_SHARED_LIBS "Build libraries as shared as opposed to static" ON) - -# Enable RPATH Support -include(AddInstallRPATHSupport) -add_install_rpath_support(BIN_DIRS "${CMAKE_INSTALL_FULL_LIBDIR}" - LIB_DIRS "${CMAKE_INSTALL_FULL_BINDIR}" - INSTALL_NAME_DIR "${CMAKE_INSTALL_FULL_LIBDIR}" - USE_LINK_PATH) - -# Build in pure c++11 -set(CMAKE_CXX_EXTENSIONS OFF) -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -# Build position independent code -set(CMAKE_POSITION_INDEPENDENT_CODE ON) - -# Hide all symbols by default -set(CMAKE_CXX_VISIBILITY_PRESET hidden) -set(CMAKE_VISIBILITY_INLINES_HIDDEN 1) - -add_subdirectory(src) - -# Install CMake config files for the library -include(InstallBasicPackageFiles) -install_basic_package_files(shlibpp - VERSION ${shlibpp_VERSION} - COMPATIBILITY AnyNewerVersion - EXPORT shlibpp - NO_CHECK_REQUIRED_COMPONENTS_MACRO) - -# Add uninstall target -# include(AddUninstallTarget) - -# Do not put tests and examples in the same output directories as the main -# targets -unset(CMAKE_RUNTIME_OUTPUT_DIRECTORY) -unset(CMAKE_LIBRARY_OUTPUT_DIRECTORY) -unset(CMAKE_ARCHIVE_OUTPUT_DIRECTORY) - - -# Create examples -option(BUILD_SHAREDLIBPP_EXAMPLES "Build examples" OFF) -mark_as_advanced(BUILD_SHAREDLIBPP_EXAMPLES) -if(BUILD_SHAREDLIBPP_EXAMPLES) - add_subdirectory(examples) -endif() diff --git a/deps/sharedlibpp/LICENSE b/deps/sharedlibpp/LICENSE deleted file mode 100644 index 9f664c855..000000000 --- a/deps/sharedlibpp/LICENSE +++ /dev/null @@ -1,26 +0,0 @@ -Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: -1. Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. -2. Redistributions in binary form must reproduce the above copyright - notice, this list of conditions and the following disclaimer in the - documentation and/or other materials provided with the distribution. -3. Neither the name of the University nor the names of its contributors - may be used to endorse or promote products derived from this software - without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND -ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -ARE DISCLAIMED. IN NO EVENT SHALL THE REGENTS OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS -OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) -HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY -OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF -SUCH DAMAGE. diff --git a/deps/sharedlibpp/README.md b/deps/sharedlibpp/README.md deleted file mode 100644 index a00f49212..000000000 --- a/deps/sharedlibpp/README.md +++ /dev/null @@ -1,45 +0,0 @@ -Simple cross platform plug-in system -==================================== - -The sharedlibpp is a tiny cross-platform library to create and load shared -libraries for different platform (Linux/Mac/Windows). The sharedlibpp provide -an easy and portable way to create plug-ins which encapsulate your c++ classes -inside a shared library (so, dylib, dll). -The original code is taken and from -[YARP (Yet Another Robot Platform)](http://www.yarp.it/). -The code is simplified by removing dependencies and some helper functions are -added to report the native OS error messages on failures. - - -Building on Linux/Mac ---------------------- - $ cd sharedlibpp - $ mkdir build; cd build - $ cmake ../; make - - -Building on Windows -------------------- -The easiest way is to use Cmake to create VStudio project. To do that: - -* download and install [cmake](http://www.cmake.org/download/) for windows. -* open the cmake gui and set the source path to `sharedlibpp` and set the - build path (for example `sharedlibpp/build`). -* configure and generate visual studio project. -* open the project and built it. - -Running example ---------------- -The build system by defualt compiles and build the examples. - -* On Linux/Mac -``` - $ cd sharedlibpp/build/examples - $ ./math_test mymath.so - $ ./math_test_custom mymathcustom.so -``` -* On Windows first switch to directory where example is created and then -``` - > math_test mymath.dll - > math_test_custom mymathcustom.dll -``` diff --git a/deps/sharedlibpp/cmake/AddInstallRPATHSupport.cmake b/deps/sharedlibpp/cmake/AddInstallRPATHSupport.cmake deleted file mode 100644 index c36cc2072..000000000 --- a/deps/sharedlibpp/cmake/AddInstallRPATHSupport.cmake +++ /dev/null @@ -1,169 +0,0 @@ -#.rst: -# AddInstallRPATHSupport -# ---------------------- -# -# Add support to RPATH during installation to your project:: -# -# add_install_rpath_support([BIN_DIRS dir [dir]] -# [LIB_DIRS dir [dir]] -# [INSTALL_NAME_DIR [dir]] -# [DEPENDS condition [condition]] -# [USE_LINK_PATH]) -# -# Normally (depending on the platform) when you install a shared -# library you can either specify its absolute path as the install name, -# or leave just the library name itself. In the former case the library -# will be correctly linked during run time by all executables and other -# shared libraries, but it must not change its install location. This -# is often the case for libraries installed in the system default -# library directory (e.g. ``/usr/lib``). -# In the latter case, instead, the library can be moved anywhere in the -# file system but at run time the dynamic linker must be able to find -# it. This is often accomplished by setting environmental variables -# (i.e. ``LD_LIBRARY_PATH`` on Linux). -# This procedure is usually not desirable for two main reasons: -# -# - by setting the variable you are changing the default behaviour -# of the dynamic linker thus potentially breaking executables (not as -# destructive as ``LD_PRELOAD``) -# - the variable will be used only by applications spawned by the shell -# and not by other processes. -# -# RPATH aims in solving the issues introduced by the second -# installation method. Using run-path dependent libraries you can -# create a directory structure containing executables and dependent -# libraries that users can relocate without breaking it. -# A run-path dependent library is a dependent library whose complete -# install name is not known when the library is created. -# Instead, the library specifies that the dynamic loader must resolve -# the library’s install name when it loads the executable that depends -# on the library. The executable or the other shared library will -# hardcode in the binary itself the additional search directories -# to be passed to the dynamic linker. This works great in conjunction -# with relative paths. -# This command will enable support to RPATH to your project. -# It will enable the following things: -# -# - If the project builds shared libraries it will generate a run-path -# enabled shared library, i.e. its install name will be resolved -# only at run time. -# - In all cases (building executables and/or shared libraries) -# dependent shared libraries with RPATH support will have their name -# resolved only at run time, by embedding the search path directly -# into the built binary. -# -# The command has the following parameters: -# -# Options: -# - ``USE_LINK_PATH``: if passed the command will automatically adds to -# the RPATH the path to all the dependent libraries. -# -# Arguments: -# - ``BIN_DIRS`` list of directories when the targets (executable and -# plugins) will be installed. -# - ``LIB_DIRS`` list of directories to be added to the RPATH. These -# directories will be added "relative" w.r.t. the ``BIN_DIRS`` and -# ``LIB_DIRS``. -# - ``INSTALL_NAME_DIR`` directory where the libraries will be installed. -# This variable will be used only if ``CMAKE_SKIP_RPATH`` or -# ``CMAKE_SKIP_INSTALL_RPATH`` is set to ``TRUE`` as it will set the -# ``INSTALL_NAME_DIR`` on all targets -# - ``DEPENDS`` list of conditions that should be ``TRUE`` to enable -# RPATH, for example ``FOO; NOT BAR``. -# -# Note: see https://gitlab.kitware.com/cmake/cmake/issues/16589 for further -# details. - -#======================================================================= -# Copyright 2014 Istituto Italiano di Tecnologia (IIT) -# @author Francesco Romano -# -# 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.) - - -include(CMakeParseArguments) - - -function(ADD_INSTALL_RPATH_SUPPORT) - - set(_options USE_LINK_PATH) - set(_oneValueArgs INSTALL_NAME_DIR) - set(_multiValueArgs BIN_DIRS - LIB_DIRS - DEPENDS) - - cmake_parse_arguments(_ARS "${_options}" - "${_oneValueArgs}" - "${_multiValueArgs}" - "${ARGN}") - - # if either RPATH or INSTALL_RPATH is disabled - # and the INSTALL_NAME_DIR variable is set, then hardcode the install name - if(CMAKE_SKIP_RPATH OR CMAKE_SKIP_INSTALL_RPATH) - if(DEFINED _ARS_INSTALL_NAME_DIR) - set(CMAKE_INSTALL_NAME_DIR ${_ARS_INSTALL_NAME_DIR} PARENT_SCOPE) - endif() - endif() - - if (CMAKE_SKIP_RPATH OR (CMAKE_SKIP_INSTALL_RPATH AND CMAKE_SKIP_BUILD_RPATH)) - return() - endif() - - - set(_rpath_available 1) - if(DEFINED _ARS_DEPENDS) - foreach(_dep ${_ARS_DEPENDS}) - string(REGEX REPLACE " +" ";" _dep "${_dep}") - if(NOT (${_dep})) - set(_rpath_available 0) - endif() - endforeach() - endif() - - if(_rpath_available) - - # Enable RPATH on OSX. - set(CMAKE_MACOSX_RPATH TRUE PARENT_SCOPE) - - # Find system implicit lib directories - set(_system_lib_dirs ${CMAKE_PLATFORM_IMPLICIT_LINK_DIRECTORIES}) - if(EXISTS "/etc/debian_version") # is this a debian system ? - if(CMAKE_LIBRARY_ARCHITECTURE) - list(APPEND _system_lib_dirs "/lib/${CMAKE_LIBRARY_ARCHITECTURE}" - "/usr/lib/${CMAKE_LIBRARY_ARCHITECTURE}") - endif() - endif() - # This is relative RPATH for libraries built in the same project - foreach(lib_dir ${_ARS_LIB_DIRS}) - list(FIND _system_lib_dirs "${lib_dir}" isSystemDir) - if("${isSystemDir}" STREQUAL "-1") - foreach(bin_dir ${_ARS_LIB_DIRS} ${_ARS_BIN_DIRS}) - file(RELATIVE_PATH _rel_path ${bin_dir} ${lib_dir}) - if (${CMAKE_SYSTEM_NAME} MATCHES "Darwin") - list(APPEND CMAKE_INSTALL_RPATH "@loader_path/${_rel_path}") - else() - list(APPEND CMAKE_INSTALL_RPATH "\$ORIGIN/${_rel_path}") - endif() - endforeach() - endif() - endforeach() - if(NOT "${CMAKE_INSTALL_RPATH}" STREQUAL "") - list(REMOVE_DUPLICATES CMAKE_INSTALL_RPATH) - endif() - set(CMAKE_INSTALL_RPATH ${CMAKE_INSTALL_RPATH} PARENT_SCOPE) - - # add the automatically determined parts of the RPATH - # which point to directories outside the build tree to the install RPATH - set(CMAKE_INSTALL_RPATH_USE_LINK_PATH ${_ARS_USE_LINK_PATH} PARENT_SCOPE) - - endif() - -endfunction() diff --git a/deps/sharedlibpp/cmake/AddUninstallTarget.cmake b/deps/sharedlibpp/cmake/AddUninstallTarget.cmake deleted file mode 100644 index fefef2d1e..000000000 --- a/deps/sharedlibpp/cmake/AddUninstallTarget.cmake +++ /dev/null @@ -1,70 +0,0 @@ -#.rst: -# AddUninstallTarget -# ------------------ -# -# Add the "uninstall" target for your project:: -# -# include(AddUninstallTarget) -# -# -# will create a file cmake_uninstall.cmake in the build directory and add a -# custom target uninstall that will remove the files installed by your package -# (using install_manifest.txt) - -#============================================================================= -# Copyright 2008-2013 Kitware, Inc. -# Copyright 2013 Istituto Italiano di Tecnologia (IIT) -# Authors: Daniele E. Domenichelli -# -# 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.) - - -if(DEFINED __ADD_UNINSTALL_TARGET_INCLUDED) - return() -endif() -set(__ADD_UNINSTALL_TARGET_INCLUDED TRUE) - - -set(_filename ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) - -file(WRITE ${_filename} -"if(NOT EXISTS \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\") - message(WARNING \"Cannot find install manifest: \\\"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\\\"\") - return() -endif() - -file(READ \"${CMAKE_CURRENT_BINARY_DIR}/install_manifest.txt\" files) -string(STRIP \"\${files}\" files) -string(REGEX REPLACE \"\\n\" \";\" files \"\${files}\") -list(REVERSE files) -foreach(file \${files}) - message(STATUS \"Uninstalling: \$ENV{DESTDIR}\${file}\") - if(EXISTS \"\$ENV{DESTDIR}\${file}\") - execute_process( - COMMAND \${CMAKE_COMMAND} -E remove \"\$ENV{DESTDIR}\${file}\" - OUTPUT_VARIABLE rm_out - RESULT_VARIABLE rm_retval) - if(NOT \"\${rm_retval}\" EQUAL 0) - message(FATAL_ERROR \"Problem when removing \\\"\$ENV{DESTDIR}\${file}\\\"\") - endif() - else() - message(STATUS \"File \\\"\$ENV{DESTDIR}\${file}\\\" does not exist.\") - endif() -endforeach(file) -") - -if("${CMAKE_GENERATOR}" MATCHES "^(Visual Studio|Xcode)") - set(_uninstall "UNINSTALL") -else() - set(_uninstall "uninstall") -endif() -add_custom_target(${_uninstall} COMMAND "${CMAKE_COMMAND}" -P "${_filename}") -set_property(TARGET ${_uninstall} PROPERTY FOLDER "CMakePredefinedTargets") diff --git a/deps/sharedlibpp/cmake/InstallBasicPackageFiles.cmake b/deps/sharedlibpp/cmake/InstallBasicPackageFiles.cmake deleted file mode 100644 index aa216a728..000000000 --- a/deps/sharedlibpp/cmake/InstallBasicPackageFiles.cmake +++ /dev/null @@ -1,677 +0,0 @@ -#.rst: -# InstallBasicPackageFiles -# ------------------------ -# -# A helper module to make your package easier to be found by other -# projects. -# -# -# .. command:: install_basic_package_files -# -# Create and install a basic version of cmake config files for your -# project:: -# -# install_basic_package_files( -# VERSION -# COMPATIBILITY -# [EXPORT ] # (default = "") -# [FIRST_TARGET ] # (default = "") -# [TARGETS ...] -# [TARGETS_PROPERTY ] -# [TARGETS_PROPERTIES ...] -# [NO_SET_AND_CHECK_MACRO] -# [NO_CHECK_REQUIRED_COMPONENTS_MACRO] -# [VARS_PREFIX ] # (default = "") -# [EXPORT_DESTINATION ] -# [INSTALL_DESTINATION ] -# [NAMESPACE ] # (default = "::") -# [EXTRA_PATH_VARS_SUFFIX path1 [path2 ...]] -# [CONFIG_TEMPLATE ] -# [UPPERCASE_FILENAMES | LOWERCASE_FILENAMES] -# [DEPENDENCIES " [...]" ...] -# [PRIVATE_DEPENDENCIES " [...]" ...] -# [INCLUDE_FILE ] -# [COMPONENT ] # (default = "") -# [NO_COMPATIBILITY_VARS] -# ) -# -# Depending on UPPERCASE_FILENAMES and LOWERCASE_FILENAMES, this -# function generates 3 files: -# -# - ``ConfigVersion.cmake`` or ``-config-version.cmake`` -# - ``Config.cmake`` or ``-config.cmake`` -# - ``Targets.cmake`` or ``-targets.cmake`` -# -# If neither ``UPPERCASE_FILENAMES`` nor ``LOWERCASE_FILENAMES`` is -# set, a file ``ConfigVersion.cmake.in`` or -# ``-config-version.cmake.in`` is searched, and the convention -# is chosed according to the file found. If no file was found, the -# uppercase convention is used. -# -# The ``DEPENDENCIES`` argument can be used to set a list of dependencies -# that will be searched using the :command:`find_dependency` command -# from the :module:`CMakeFindDependencyMacro` module. -# Dependencies can be followed by any of the possible :command:`find_dependency` -# argument. -# In this case, all the arguments must be specified within double quotes (e.g. -# " 1.0.0 EXACT", " CONFIG"). -# The ``PRIVATE_DEPENDENCIES`` argument is similar to ``DEPENDENCIES``, but -# these dependencies are included only when libraries are built ``STATIC``, i.e. -# if ``BUILD_SHARED_LIBS`` is ``OFF`` or if the ``TYPE`` property for one or -# more of the targets is ``STATIC_LIBRARY``. -# When using a custom template file, the ``@PACKAGE_DEPENDENCIES@`` -# string is replaced with the code checking for the dependencies -# specified by these two argument. -# -# Each file is generated twice, one for the build directory and one for -# the installation directory. The ``INSTALL_DESTINATION`` argument can be -# passed to install the files in a location different from the default -# one (``CMake`` on Windows, ``${CMAKE_INSTALL_LIBDIR}/cmake/${Name}`` -# on other platforms. The ``EXPORT_DESTINATION`` argument can be passed to -# generate the files in the build tree in a location different from the default -# one (``CMAKE_BINARY_DIR``). If this is a relative path, it is considered -# relative to the ``CMAKE_BINARY_DIR`` directory. -# -# The ``ConfigVersion.cmake`` is generated using -# ``write_basic_package_version_file``. The ``VERSION``, -# ``COMPATIBILITY``, ``NO_SET_AND_CHECK_MACRO``, and -# ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` are passed to this function -# and are used internally by :module:`CMakePackageConfigHelpers` module. -# -# ``VERSION`` shall be in the form ``[.[.[.]]]]``. -# If no ``VERSION`` is given, the ``PROJECT_VERSION`` variable is used. -# If this hasn’t been set, it errors out. The ``VERSION`` argument is also used -# to replace the ``@PACKAGE_VERSION@`` string in the configuration file. -# -# ``COMPATIBILITY`` shall be any of ````. -# The ``COMPATIBILITY`` mode ``AnyNewerVersion`` means that the installed -# package version will be considered compatible if it is newer or exactly the -# same as the requested version. This mode should be used for packages which are -# fully backward compatible, also across major versions. -# If ``SameMajorVersion`` is used instead, then the behaviour differs from -# ``AnyNewerVersion`` in that the major version number must be the same as -# requested, e.g. version 2.0 will not be considered compatible if 1.0 is -# requested. This mode should be used for packages which guarantee backward -# compatibility within the same major version. If ``ExactVersion`` is used, then -# the package is only considered compatible if the requested version matches -# exactly its own version number (not considering the tweak version). For -# example, version 1.2.3 of a package is only considered compatible to requested -# version 1.2.3. This mode is for packages without compatibility guarantees. If -# your project has more elaborated version matching rules, you will need to -# write your own custom ConfigVersion.cmake file instead of using this macro. -# -# By default ``install_basic_package_files`` also generates the two helper -# macros ``set_and_check()`` and ``check_required_components()`` into the -# ``Config.cmake`` file. ``set_and_check()`` should be used instead of the -# normal set() command for setting directories and file locations. Additionally -# to setting the variable it also checks that the referenced file or directory -# actually exists and fails with a ``FATAL_ERROR`` otherwise. This makes sure -# that the created ``Config.cmake`` file does not contain wrong -# references. When using the ``NO_SET_AND_CHECK_MACRO, this macro is not -# generated into the ``Config.cmake`` file. -# -# By default, ``install_basic_package_files`` append a call to -# ``check_required_components()`` in Config.cmake file if the -# package supports components. This macro checks whether all requested, -# non-optional components have been found, and if this is not the case, sets the -# ``_FOUND`` variable to ``FALSE``, so that the package is considered to -# be not found. It does that by testing the ``__FOUND`` -# variables for all requested required components. When using the -# ``NO_CHECK_REQUIRED_COMPONENTS_MACRO`` option, this macro is not generated -# into the Config.cmake file. -# -# Finally, the files in the build and install directory are exactly the same. -# -# See the documentation of :module:`CMakePackageConfigHelpers` module for -# further information and references therein. -# -# -# The ``Config.cmake`` is generated using -# ``configure_package_config_file``. See the documentation for the -# :module:`CMakePackageConfigHelpers` module for further information. -# If the ``CONFIG_TEMPLATE`` argument is passed, the specified file -# is used as template for generating the configuration file, otherwise -# this module expects to find a ``Config.cmake.in`` or -# ``-config.cmake.in`` file either in the root directory of the -# project or in current source directory. -# If the file does not exist, a very basic file is created. -# -# A set of variables are checked and passed to -# ``configure_package_config_file`` as ``PATH_VARS``. For each of the -# ``SUFFIX`` considered, if one of the variables:: -# -# _(BUILD|INSTALL)_ -# (BUILD|INSTALL)__ -# -# is defined, the ``_`` variable will be defined -# before configuring the package. In order to use that variable in the -# config file, you have to add a line:: -# -# set_and_check(_ \"@PACKAGE__@\") -# -# if the path must exist or just:: -# -# set(_ \"@PACKAGE__@\") -# -# if the path could be missing. -# -# These variable will have different values whether you are using the -# package from the build tree or from the install directory. Also these -# files will contain only relative paths, meaning that you can move the -# whole installation and the CMake files will still work. -# -# Default ``PATH_VARS`` suffixes are:: -# -# BINDIR BIN_DIR -# SBINDIR SBIN_DIR -# LIBEXECDIR LIBEXEC_DIR -# SYSCONFDIR SYSCONF_DIR -# SHAREDSTATEDIR SHAREDSTATE_DIR -# LOCALSTATEDIR LOCALSTATE_DIR -# LIBDIR LIB_DIR -# INCLUDEDIR INCLUDE_DIR -# OLDINCLUDEDIR OLDINCLUDE_DIR -# DATAROOTDIR DATAROOT_DIR -# DATADIR DATA_DIR -# INFODIR INFO_DIR -# LOCALEDIR LOCALE_DIR -# MANDIR MAN_DIR -# DOCDIR DOC_DIR -# -# more suffixes can be added using the ``EXTRA_PATH_VARS_SUFFIX`` -# argument. -# -# -# The ``Targets.cmake`` is generated using -# :command:`export(TARGETS)` (if ``EXPORT`` or no options are used) or -# :command:`export(TARGETS)` (if `EXPORT` is not used and one between -# ``TARGETS``, ``TARGETS_PROPERTY``, or ``TARGETS_PROPERTIES`` is used) in the -# build tree and :command:`install(EXPORT)` in the installation directory. -# The targets are exported using the value for the ``NAMESPACE`` -# argument as namespace. -# The export can be passed using the `EXPORT` argument. -# The targets can be passed using the `TARGETS` argument or using one or more -# global properties, that can be passed to the function using the -# ``TARGETS_PROPERTY`` or ``TARGET_PROPERTIES`` arguments. -# -# If the ``NO_COMPATIBILITY_VARS`` argument is not set, the compatibility -# variables ``_LIBRARIES`` and ``_INCLUDE_DIRS`` -# are set, trying to guess their correct values from the variables set or -# from the arguments passed to this command. This argument is ignored if -# the template file is not generated by this command. -# -# If the ``INCLUDE_FILE`` argument is passed, the content of the specified file -# (which might be templated) is appended to the ``Config.cmake``. -# This allows to inject custom code to this file, useful e.g. to set additional -# variables which are loaded by downstream projects. -# -# If the ``COMPONENT`` argument is passed, it is forwarded to the -# :command:`install` commands, otherwise is used. - -#============================================================================= -# Copyright 2013 Istituto Italiano di Tecnologia (IIT) -# Authors: Daniele E. Domenichelli -# -# 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.) - - -if(COMMAND install_basic_package_files) - return() -endif() - - -include(GNUInstallDirs) -include(CMakePackageConfigHelpers) -include(CMakeParseArguments) - - -function(INSTALL_BASIC_PACKAGE_FILES _Name) - - # TODO check that _Name does not contain "-" characters - - set(_options NO_SET_AND_CHECK_MACRO - NO_CHECK_REQUIRED_COMPONENTS_MACRO - UPPERCASE_FILENAMES - LOWERCASE_FILENAMES - NO_COMPATIBILITY_VARS) - set(_oneValueArgs VERSION - COMPATIBILITY - EXPORT - FIRST_TARGET - TARGETS_PROPERTY - VARS_PREFIX - EXPORT_DESTINATION - INSTALL_DESTINATION - DESTINATION - NAMESPACE - CONFIG_TEMPLATE - INCLUDE_FILE - COMPONENT) - set(_multiValueArgs EXTRA_PATH_VARS_SUFFIX - TARGETS - TARGETS_PROPERTIES - DEPENDENCIES - PRIVATE_DEPENDENCIES) - cmake_parse_arguments(_IBPF "${_options}" "${_oneValueArgs}" "${_multiValueArgs}" "${ARGN}") - - if(NOT DEFINED _IBPF_VARS_PREFIX) - set(_IBPF_VARS_PREFIX ${_Name}) - endif() - - if(NOT DEFINED _IBPF_VERSION) - message(FATAL_ERROR "VERSION argument is required") - endif() - - if(NOT DEFINED _IBPF_COMPATIBILITY) - message(FATAL_ERROR "COMPATIBILITY argument is required") - endif() - - if(_IBPF_UPPERCASE_FILENAMES AND _IBPF_LOWERCASE_FILENAMES) - message(FATAL_ERROR "UPPERCASE_FILENAMES and LOWERCASE_FILENAMES arguments cannot be used together") - endif() - - # Prepare install and export commands - set(_first_target ${_Name}) - set(_targets ${_Name}) - set(_install_cmd EXPORT ${_Name}) - set(_export_cmd EXPORT ${_Name}) - - if(DEFINED _IBPF_FIRST_TARGET) - if(DEFINED _IBPF_TARGETS OR DEFINED _IBPF_TARGETS_PROPERTIES OR DEFINED _IBPF_TARGETS_PROPERTIES) - message(FATAL_ERROR "EXPORT cannot be used with TARGETS, TARGETS_PROPERTY or TARGETS_PROPERTIES") - endif() - - set(_first_target ${_IBPF_FIRST_TARGET}) - set(_targets ${_IBPF_FIRST_TARGET}) - endif() - - if(DEFINED _IBPF_EXPORT) - if(DEFINED _IBPF_TARGETS OR DEFINED _IBPF_TARGETS_PROPERTIES OR DEFINED _IBPF_TARGETS_PROPERTIES) - message(FATAL_ERROR "EXPORT cannot be used with TARGETS, TARGETS_PROPERTY or TARGETS_PROPERTIES") - endif() - - set(_export_cmd EXPORT ${_IBPF_EXPORT}) - set(_install_cmd EXPORT ${_IBPF_EXPORT}) - - elseif(DEFINED _IBPF_TARGETS) - if(DEFINED _IBPF_TARGETS_PROPERTY OR DEFINED _IBPF_TARGETS_PROPERTIES) - message(FATAL_ERROR "TARGETS cannot be used with TARGETS_PROPERTY or TARGETS_PROPERTIES") - endif() - - set(_targets ${_IBPF_TARGETS}) - set(_export_cmd TARGETS ${_IBPF_TARGETS}) - list(GET _targets 0 _first_target) - - elseif(DEFINED _IBPF_TARGETS_PROPERTY) - if(DEFINED _IBPF_TARGETS_PROPERTIES) - message(FATAL_ERROR "TARGETS_PROPERTIES cannot be used with TARGETS_PROPERTIES") - endif() - - get_property(_targets GLOBAL PROPERTY ${_IBPF_TARGETS_PROPERTY}) - set(_export_cmd TARGETS ${_targets}) - list(GET _targets 0 _first_target) - - elseif(DEFINED _IBPF_TARGETS_PROPERTIES) - - unset(_targets) - foreach(_prop ${_IBPF_TARGETS_PROPERTIES}) - get_property(_prop_val GLOBAL PROPERTY ${_prop}) - list(APPEND _targets ${_prop_val}) - endforeach() - set(_export_cmd TARGETS ${_targets}) - list(GET _targets 0 _first_target) - - endif() - - # Path for installed cmake files - if(DEFINED _IBPF_DESTINATION) - message(DEPRECATION "DESTINATION is deprecated. Use INSTALL_DESTINATION instead") - if(NOT DEFINED _IBPF_INSTALL_DESTINATION) - set(_IBPF_INSTALL_DESTINATION ${_IBPF_DESTINATION}) - endif() - endif() - - if(NOT DEFINED _IBPF_INSTALL_DESTINATION) - if(WIN32 AND NOT CYGWIN) - set(_IBPF_INSTALL_DESTINATION CMake) - else() - set(_IBPF_INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${_Name}) - endif() - endif() - - if(NOT DEFINED _IBPF_EXPORT_DESTINATION) - set(_IBPF_EXPORT_DESTINATION "${CMAKE_BINARY_DIR}") - elseif(NOT IS_ABSOLUTE _IBPF_EXPORT_DESTINATION) - set(_IBPF_EXPORT_DESTINATION "${CMAKE_BINARY_DIR}/${_IBPF_EXPORT_DESTINATION}") - endif() - - if(NOT DEFINED _IBPF_NAMESPACE) - set(_IBPF_NAMESPACE "${_Name}::") - endif() - - if(NOT DEFINED _IBPF_COMPONENT) - set(_IBPF_COMPONENT "${_Name}") - endif() - - if(_IBPF_NO_SET_AND_CHECK_MACRO) - list(APPEND configure_package_config_file_extra_args NO_SET_AND_CHECK_MACRO) - endif() - - if(_IBPF_NO_CHECK_REQUIRED_COMPONENTS_MACRO) - list(APPEND configure_package_config_file_extra_args NO_CHECK_REQUIRED_COMPONENTS_MACRO) - endif() - - - - # Set input file for config, and ensure that _IBPF_UPPERCASE_FILENAMES - # and _IBPF_LOWERCASE_FILENAMES are set correctly - unset(_config_cmake_in) - set(_generate_file 0) - if(DEFINED _IBPF_CONFIG_TEMPLATE) - if(NOT EXISTS "${_IBPF_CONFIG_TEMPLATE}") - message(FATAL_ERROR "Config template file \"${_IBPF_CONFIG_TEMPLATE}\" not found") - endif() - set(_config_cmake_in "${_IBPF_CONFIG_TEMPLATE}") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - if("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_Name}Config.cmake.in") - set(_IBPF_UPPERCASE_FILENAMES 1) - elseif("${_IBPF_CONFIG_TEMPLATE}" MATCHES "${_name}-config.cmake.in") - set(_IBPF_LOWERCASE_FILENAMES 1) - else() - set(_IBPF_UPPERCASE_FILENAMES 1) - endif() - endif() - else() - string(TOLOWER "${_Name}" _name) - if(EXISTS "${CMAKE_SOURCE_DIR}/${_Name}Config.cmake.in") - set(_config_cmake_in "${CMAKE_SOURCE_DIR}/${_Name}Config.cmake.in") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - set(_IBPF_UPPERCASE_FILENAMES 1) - endif() - elseif(EXISTS "${CMAKE_SOURCE_DIR}/${_name}-config.cmake.in") - set(_config_cmake_in "${CMAKE_SOURCE_DIR}/${_name}-config.cmake.in") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - set(_IBPF_LOWERCASE_FILENAMES 1) - endif() - elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in") - set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_Name}Config.cmake.in") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - set(_IBPF_UPPERCASE_FILENAMES 1) - endif() - elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in") - set(_config_cmake_in "${CMAKE_CURRENT_SOURCE_DIR}/${_name}-config.cmake.in") - if(NOT _IBPF_UPPERCASE_FILENAMES AND NOT _IBPF_LOWERCASE_FILENAMES) - set(_IBPF_LOWERCASE_FILENAMES 1) - endif() - else() - set(_generate_file 1) - if(_IBPF_LOWERCASE_FILENAMES) - set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_name}-config.cmake") - else() - set(_config_cmake_in "${CMAKE_CURRENT_BINARY_DIR}/${_Name}Config.cmake.in") - set(_IBPF_UPPERCASE_FILENAMES 1) - endif() - endif() - endif() - - # Set input file containing user variables - if(DEFINED _IBPF_INCLUDE_FILE) - if(NOT IS_ABSOLUTE "${_IBPF_INCLUDE_FILE}") - if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}") - set(_IBPF_INCLUDE_FILE "${CMAKE_CURRENT_SOURCE_DIR}/${_IBPF_INCLUDE_FILE}") - endif() - endif() - if(NOT EXISTS "${_IBPF_INCLUDE_FILE}") - message(FATAL_ERROR "File \"${_IBPF_INCLUDE_FILE}\" not found") - endif() - file(READ ${_IBPF_INCLUDE_FILE} _includedfile_user_content_in) - string(CONFIGURE ${_includedfile_user_content_in} _includedfile_user_content) - set(INCLUDED_FILE_CONTENT -"#### Expanded from INCLUDE_FILE by install_basic_package_files() ####") - set(INCLUDED_FILE_CONTENT "${INCLUDED_FILE_CONTENT}\n\n${_includedfile_user_content}") - set(INCLUDED_FILE_CONTENT -"${INCLUDED_FILE_CONTENT} -#####################################################################") - endif() - - # Select output file names - if(_IBPF_UPPERCASE_FILENAMES) - set(_config_filename ${_Name}Config.cmake) - set(_version_filename ${_Name}ConfigVersion.cmake) - set(_targets_filename ${_Name}Targets.cmake) - elseif(_IBPF_LOWERCASE_FILENAMES) - set(_config_filename ${_name}-config.cmake) - set(_version_filename ${_name}-config-version.cmake) - set(_targets_filename ${_name}-targets.cmake) - endif() - - - # If the template config file does not exist, write a basic one - if(_generate_file) - # Generate the compatibility code - unset(_compatibility_vars) - if(NOT _IBPF_NO_COMPATIBILITY_VARS) - unset(_get_include_dir_code) - unset(_set_include_dir_code) - unset(_target_list) - foreach(_target ${_targets}) - list(APPEND _target_list ${_IBPF_NAMESPACE}${_target}) - endforeach() - if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDEDIR OR - DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDEDIR OR - DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDEDIR OR - DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDEDIR) - set(_get_include_dir "set(${_IBPF_VARS_PREFIX}_INCLUDEDIR \"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDEDIR\@\")\n") - set(_set_include_dir "set(${_Name}_INCLUDE_DIRS \"\${${_IBPF_VARS_PREFIX}_INCLUDEDIR}\")") - elseif(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_INCLUDE_DIR OR - DEFINED BUILD_${_IBPF_VARS_PREFIX}_INCLUDE_DIR OR - DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_INCLUDE_DIR OR - DEFINED INSTALL_${_IBPF_VARS_PREFIX}_INCLUDE_DIR) - set(_get_include_dir "set(${_IBPF_VARS_PREFIX}_INCLUDE_DIR \"\@PACKAGE_${_IBPF_VARS_PREFIX}_INCLUDE_DIR\@\")\n") - set(_set_include_dir "set(${_Name}_INCLUDE_DIRS \"\${${_IBPF_VARS_PREFIX}_INCLUDE_DIR}\")") - else() - unset(_include_dir_list) - foreach(_target ${_targets}) - set(_get_include_dir "${_get_include_dir}get_property(${_IBPF_VARS_PREFIX}_${_target}_INCLUDE_DIR TARGET ${_IBPF_NAMESPACE}${_target} PROPERTY INTERFACE_INCLUDE_DIRECTORIES)\n") - list(APPEND _include_dir_list "\"\${${_IBPF_VARS_PREFIX}_${_target}_INCLUDE_DIR}\"") - endforeach() - string(REPLACE ";" " " _include_dir_list "${_include_dir_list}") - string(REPLACE ";" " " _target_list "${_target_list}") - set(_set_include_dir "set(${_Name}_INCLUDE_DIRS ${_include_dir_list})\nlist(REMOVE_DUPLICATES ${_Name}_INCLUDE_DIRS)") - endif() - set(_compatibility_vars "# Compatibility\n${_get_include_dir}\nset(${_Name}_LIBRARIES ${_target_list})\n${_set_include_dir}") - endif() - - # Write the file - file(WRITE "${_config_cmake_in}" -"set(${_IBPF_VARS_PREFIX}_VERSION \@PACKAGE_VERSION\@) - -\@PACKAGE_INIT\@ - -\@PACKAGE_DEPENDENCIES\@ - -if(NOT TARGET ${_IBPF_NAMESPACE}${_first_target}) - include(\"\${CMAKE_CURRENT_LIST_DIR}/${_targets_filename}\") -endif() - -${_compatibility_vars} - -\@INCLUDED_FILE_CONTENT\@ -") - endif() - - # Make relative paths absolute (needed later on) and append the - # defined variables to _(build|install)_path_vars_suffix - foreach(p BINDIR BIN_DIR - SBINDIR SBIN_DIR - LIBEXECDIR LIBEXEC_DIR - SYSCONFDIR SYSCONF_DIR - SHAREDSTATEDIR SHAREDSTATE_DIR - LOCALSTATEDIR LOCALSTATE_DIR - LIBDIR LIB_DIR - INCLUDEDIR INCLUDE_DIR - OLDINCLUDEDIR OLDINCLUDE_DIR - DATAROOTDIR DATAROOT_DIR - DATADIR DATA_DIR - INFODIR INFO_DIR - LOCALEDIR LOCALE_DIR - MANDIR MAN_DIR - DOCDIR DOC_DIR - ${_IBPF_EXTRA_PATH_VARS_SUFFIX}) - if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p}) - list(APPEND _build_path_vars_suffix ${p}) - list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - if(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p}) - list(APPEND _build_path_vars_suffix ${p}) - list(APPEND _build_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p}) - list(APPEND _install_path_vars_suffix ${p}) - list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - if(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p}) - list(APPEND _install_path_vars_suffix ${p}) - list(APPEND _install_path_vars "${_IBPF_VARS_PREFIX}_${p}") - endif() - endforeach() - - - # ConfigVersion.cmake file (same for build tree and intall) - write_basic_package_version_file("${_IBPF_EXPORT_DESTINATION}/${_version_filename}" - VERSION ${_IBPF_VERSION} - COMPATIBILITY ${_IBPF_COMPATIBILITY}) - install(FILES "${_IBPF_EXPORT_DESTINATION}/${_version_filename}" - DESTINATION ${_IBPF_INSTALL_DESTINATION} - COMPONENT ${_IBPF_COMPONENT}) - - - # Prepare PACKAGE_DEPENDENCIES variable - set(_need_private_deps 0) - if(NOT BUILD_SHARED_LIBS) - set(_need_private_deps 1) - else() - foreach(_target ${_targets}) - get_property(_type TARGET ${_target} PROPERTY TYPE) - if("${_type}" STREQUAL "STATIC_LIBRARY") - set(_need_private_deps 1) - break() - endif() - endforeach() - endif() - - unset(PACKAGE_DEPENDENCIES) - if(DEFINED _IBPF_DEPENDENCIES) - set(PACKAGE_DEPENDENCIES "#### Expanded from @PACKAGE_DEPENDENCIES@ by install_basic_package_files() ####\n\ninclude(CMakeFindDependencyMacro)\n") - - # FIXME When CMake 3.9 or greater is required, remove this madness and just - # use find_dependency - if (CMAKE_VERSION VERSION_LESS 3.9) - string(APPEND PACKAGE_DEPENDENCIES " -set(_${_Name}_FIND_PARTS_REQUIRED) -if (${_Name}_FIND_REQUIRED) - set(_${_Name}_FIND_PARTS_REQUIRED REQUIRED) -endif() -set(_${_Name}_FIND_PARTS_QUIET) -if (${_Name}_FIND_QUIETLY) - set(_${_Name}_FIND_PARTS_QUIET QUIET) -endif() -") - - foreach(_dep ${_IBPF_DEPENDENCIES}) - if("${_dep}" MATCHES ".+ .+") - string(REPLACE " " ";" _dep_list "${_dep}") - list(INSERT _dep_list 1 \${_${_Name}_FIND_PARTS_QUIET} \${_${_Name}_FIND_PARTS_REQUIRED}) - string(REPLACE ";" " " _depx "${_dep_list}") - string(APPEND PACKAGE_DEPENDENCIES "find_package(${_depx})\n") - else() - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endif() - endforeach() - if(_need_private_deps) - foreach(_dep ${_IBPF_PRIVATE_DEPENDENCIES}) - if("${_dep}" MATCHES ".+ .+") - string(REPLACE " " ";" _dep_list "${_dep}") - list(INSERT _dep_list 1 \${_${_Name}_FIND_PARTS_QUIET} \${_${_Name}_FIND_PARTS_REQUIRED}) - string(REPLACE ";" "\n " _depx "${_dep_list}") - string(APPEND PACKAGE_DEPENDENCIES "find_package(${_depx})\n") - else() - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endif() - endforeach() - endif() - - else() - - foreach(_dep ${_IBPF_DEPENDENCIES}) - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endforeach() - if(_need_private_deps) - foreach(_dep ${_IBPF_PRIVATE_DEPENDENCIES}) - string(APPEND PACKAGE_DEPENDENCIES "find_dependency(${_dep})\n") - endforeach() - endif() - - endif() - - set(PACKAGE_DEPENDENCIES "${PACKAGE_DEPENDENCIES}\n###############################################################################\n") - endif() - - # Prepare PACKAGE_VERSION variable - set(PACKAGE_VERSION ${_IBPF_VERSION}) - - # Config.cmake (build tree) - foreach(p ${_build_path_vars_suffix}) - if(DEFINED ${_IBPF_VARS_PREFIX}_BUILD_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_BUILD_${p}}") - elseif(DEFINED BUILD_${_IBPF_VARS_PREFIX}_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${BUILD_${_IBPF_VARS_PREFIX}_${p}}") - endif() - endforeach() - configure_package_config_file("${_config_cmake_in}" - "${_IBPF_EXPORT_DESTINATION}/${_config_filename}" - INSTALL_DESTINATION ${_IBPF_EXPORT_DESTINATION} - PATH_VARS ${_build_path_vars} - ${configure_package_config_file_extra_args} - INSTALL_PREFIX ${CMAKE_BINARY_DIR}) - - # Config.cmake (installed) - foreach(p ${_install_path_vars_suffix}) - if(DEFINED ${_IBPF_VARS_PREFIX}_INSTALL_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${${_IBPF_VARS_PREFIX}_INSTALL_${p}}") - elseif(DEFINED INSTALL_${_IBPF_VARS_PREFIX}_${p}) - set(${_IBPF_VARS_PREFIX}_${p} "${INSTALL_${_IBPF_VARS_PREFIX}_${p}}") - endif() - endforeach() - configure_package_config_file("${_config_cmake_in}" - "${CMAKE_CURRENT_BINARY_DIR}/${_config_filename}.install" - INSTALL_DESTINATION ${_IBPF_INSTALL_DESTINATION} - PATH_VARS ${_install_path_vars} - ${configure_package_config_file_extra_args}) - install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${_config_filename}.install" - DESTINATION ${_IBPF_INSTALL_DESTINATION} - RENAME ${_config_filename} - COMPONENT ${_IBPF_COMPONENT}) - - - # Targets.cmake (build tree) - export(${_export_cmd} - NAMESPACE ${_IBPF_NAMESPACE} - FILE "${_IBPF_EXPORT_DESTINATION}/${_targets_filename}") - - # Targets.cmake (installed) - install(${_install_cmd} - NAMESPACE ${_IBPF_NAMESPACE} - DESTINATION ${_IBPF_INSTALL_DESTINATION} - FILE "${_targets_filename}" - COMPONENT ${_IBPF_COMPONENT}) -endfunction() diff --git a/deps/sharedlibpp/examples/CMakeLists.txt b/deps/sharedlibpp/examples/CMakeLists.txt deleted file mode 100644 index 8736b9181..000000000 --- a/deps/sharedlibpp/examples/CMakeLists.txt +++ /dev/null @@ -1,19 +0,0 @@ -# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) -# All rights reserved. -# -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. See the accompanying LICENSE file for details. - -add_library(mymath MODULE MyMath.cpp MyMath.h) -target_include_directories(mymath PRIVATE $) -set_property(TARGET mymath PROPERTY PREFIX "") - -add_executable(math_test math_test.cpp) -target_link_libraries(math_test PRIVATE shlibpp::shlibpp) - -add_library(mymathcustom MODULE MyMathCustom.cpp MyMathCustom.h) -target_include_directories(mymathcustom PRIVATE $) -set_property(TARGET mymathcustom PROPERTY PREFIX "") - -add_executable(math_test_custom math_test_custom.cpp) -target_link_libraries(math_test_custom PRIVATE shlibpp::shlibpp) diff --git a/deps/sharedlibpp/examples/MyMath.cpp b/deps/sharedlibpp/examples/MyMath.cpp deleted file mode 100644 index 8a6c99b3b..000000000 --- a/deps/sharedlibpp/examples/MyMath.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include "MyMath.h" -#include - -SHLIBPP_DEFINE_SHARED_SUBCLASS(my_math, MyMathImpl, MyMath); - -int MyMathImpl::add(int a, int b) -{ - return (a+b); -} - -int MyMathImpl::sub(int a, int b) -{ - return (a-b); -} diff --git a/deps/sharedlibpp/examples/MyMath.h b/deps/sharedlibpp/examples/MyMath.h deleted file mode 100644 index fa78441c2..000000000 --- a/deps/sharedlibpp/examples/MyMath.h +++ /dev/null @@ -1,26 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef MYMATH_H -#define MYMATH_H - -class MyMath -{ -public: - virtual int add(int a, int b) = 0; - virtual int sub(int a, int b) = 0; -}; - -class MyMathImpl : public MyMath -{ -public: - int add(int a, int b); - int sub(int a, int b); -}; - -#endif //_MYMATH_H_ diff --git a/deps/sharedlibpp/examples/MyMathCustom.cpp b/deps/sharedlibpp/examples/MyMathCustom.cpp deleted file mode 100644 index 9dd4b42cc..000000000 --- a/deps/sharedlibpp/examples/MyMathCustom.cpp +++ /dev/null @@ -1,22 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include "MyMathCustom.h" -#include - -SHLIBPP_DEFINE_SHARED_SUBCLASS_CUSTOM(CUSTOM_START_CHECK, CUSTOM_END_CHECK, CUSTOM_SYSTEM_VERSION, my_math_custom, MyMathCustomImpl, MyMathCustom); - -int MyMathCustomImpl::add(int a, int b) -{ - return (a+b); -} - -int MyMathCustomImpl::sub(int a, int b) -{ - return (a-b); -} diff --git a/deps/sharedlibpp/examples/MyMathCustom.h b/deps/sharedlibpp/examples/MyMathCustom.h deleted file mode 100644 index 98a553716..000000000 --- a/deps/sharedlibpp/examples/MyMathCustom.h +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef MYMATHCUSTOM_H -#define MYMATHCUSTOM_H - -#include - -constexpr int32_t CUSTOM_START_CHECK = - static_cast('T') + - (static_cast('E') << 8) + - (static_cast('S') << 16) + - (static_cast('T') << 24); -constexpr int32_t CUSTOM_END_CHECK = - static_cast('T') + - (static_cast('S') << 8) + - (static_cast('E') << 16) + - (static_cast('T') << 24); -constexpr int32_t CUSTOM_SYSTEM_VERSION = 42; - -class MyMathCustom -{ -public: - virtual int add(int a, int b) = 0; - virtual int sub(int a, int b) = 0; -}; - -class MyMathCustomImpl : public MyMathCustom -{ -public: - int add(int a, int b); - int sub(int a, int b); -}; - -#endif //_MYMATH_H_ diff --git a/deps/sharedlibpp/examples/math_test.cpp b/deps/sharedlibpp/examples/math_test.cpp deleted file mode 100644 index 547ad0d2e..000000000 --- a/deps/sharedlibpp/examples/math_test.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include -#include "MyMath.h" - -#include -#include - - -int main(int argc, char *argv[]) -{ - - if(argc < 2) { - printf("Usage: %s \n", argv[0]); - printf("for example: %s libmymath.so\n", argv[0]); - return 0; - } - - // create an instance of shared library class factory to load the library - printf("Loading the shared library... \n"); - shlibpp::SharedLibraryClassFactory myMathFactory(argv[1], "my_math"); - if (!myMathFactory.isValid()) { - printf("error (%d) : %s\n", static_cast(myMathFactory.getStatus()), - myMathFactory.getError().c_str()); - return 1; - } - - // create an instance of the class and call its functions - shlibpp::SharedLibraryClass myMath(myMathFactory); - printf("Calling some of its functions... \n"); - printf("15 + 12 = %d\n", myMath->add(15, 12)); - printf("15 - 12 = %d\n", myMath->sub(15, 12)); - - return 0; -} diff --git a/deps/sharedlibpp/examples/math_test_custom.cpp b/deps/sharedlibpp/examples/math_test_custom.cpp deleted file mode 100644 index bc66e63eb..000000000 --- a/deps/sharedlibpp/examples/math_test_custom.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include -#include "MyMathCustom.h" - -#include -#include - - -int main(int argc, char *argv[]) -{ - - if(argc < 2) { - printf("Usage: %s \n", argv[0]); - printf("for example: %s libmymath.so\n", argv[0]); - return 0; - } - - // create an instance of shared library class factory to load the library - printf("Loading the shared library... \n"); - shlibpp::SharedLibraryClassFactory myMathFactory(argv[1], - CUSTOM_START_CHECK, - CUSTOM_END_CHECK, - CUSTOM_SYSTEM_VERSION, - "my_math_custom"); - if (!myMathFactory.isValid()) { - printf("error (%d) : %s\n", static_cast(myMathFactory.getStatus()), - myMathFactory.getError().c_str()); - return 1; - } - - // create an instance of the class and call its functions - shlibpp::SharedLibraryClass myMath(myMathFactory); - printf("Calling some of its functions... \n"); - printf("15 + 12 = %d\n", myMath->add(15, 12)); - printf("15 - 12 = %d\n", myMath->sub(15, 12)); - - return 0; -} diff --git a/deps/sharedlibpp/src/CMakeLists.txt b/deps/sharedlibpp/src/CMakeLists.txt deleted file mode 100644 index e96cedbc4..000000000 --- a/deps/sharedlibpp/src/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -# Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) -# All rights reserved. -# -# This software may be modified and distributed under the terms of the -# BSD-3-Clause license. See the accompanying LICENSE file for details. - -include (GNUInstallDirs) - -configure_file("${CMAKE_CURRENT_SOURCE_DIR}/shlibpp/config.h.in" - "${CMAKE_CURRENT_BINARY_DIR}/shlibpp/config.h" - @ONLY) - -configure_file("${CMAKE_CURRENT_SOURCE_DIR}/shlibpp/version.h.in" - "${CMAKE_CURRENT_BINARY_DIR}/shlibpp/version.h" - @ONLY) - -set(shlibpp_HDRS "${CMAKE_CURRENT_BINARY_DIR}/shlibpp/config.h" - "${CMAKE_CURRENT_BINARY_DIR}/shlibpp/version.h" - shlibpp/api.h - shlibpp/SharedLibraryClassApi.h - shlibpp/SharedLibraryClassFactory.h - shlibpp/SharedLibraryClassFactory-inl.h - shlibpp/SharedLibraryClass.h - shlibpp/SharedLibraryClass-inl.h - shlibpp/SharedLibraryFactory.h - shlibpp/SharedLibrary.h) - -set(shlibpp_SRCS version.cpp - SharedLibrary.cpp - SharedLibraryFactory.cpp) - -add_library(shlibpp ${shlibpp_SRCS} ${shlibpp_HDRS}) -add_library(shlibpp::shlibpp ALIAS shlibpp) - -# Add build definitions -if(NOT BUILD_SHARED_LIBS) - target_compile_definitions(shlibpp PRIVATE SHLIBPP_STATIC) -endif() -set_target_properties(shlibpp PROPERTIES DEFINE_SYMBOL BUILDING_SHLIBPP) - -target_include_directories(shlibpp PUBLIC $ - $ - $) - -if(NOT CMAKE_MINIMUM_REQUIRED_VERSION VERSION_LESS 3.8) - message(AUTHOR_WARNING "CMAKE_MINIMUM_REQUIRED_VERSION is now ${CMAKE_MINIMUM_REQUIRED_VERSION}. This check can be removed") -endif() -if(CMAKE_VERSION VERSION_LESS 3.8) - # Should be enough to enable c++11 - target_compile_features(shlibpp PUBLIC cxx_constexpr - cxx_nullptr) -else() - target_compile_features(shlibpp PUBLIC cxx_std_11) -endif() - -if(UNIX) - target_link_libraries(shlibpp PRIVATE dl) -endif() - -set_property(TARGET shlibpp PROPERTY PUBLIC_HEADER ${shlibpp_HDRS}) -set_property(TARGET shlibpp PROPERTY VERSION ${shlibpp_VERSION}) -set_property(TARGET shlibpp PROPERTY SOVERSION 1) - -install(TARGETS shlibpp - EXPORT shlibpp - COMPONENT shlibpp - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/shlibpp) diff --git a/deps/sharedlibpp/src/SharedLibrary.cpp b/deps/sharedlibpp/src/SharedLibrary.cpp deleted file mode 100644 index 68d45f411..000000000 --- a/deps/sharedlibpp/src/SharedLibrary.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include - -#if defined(_WIN32) -# include -#else -# include -#endif - -#include - -using namespace shlibpp; - - -class SharedLibrary::Private -{ -public: - Private() : - implementation(nullptr), - err_message() - { - } - - void* implementation; - std::string err_message; -}; - -SharedLibrary::SharedLibrary() : - mPriv(new Private) -{ -} - -SharedLibrary::SharedLibrary(const char *filename) : - SharedLibrary() -{ - open(filename); -} - -SharedLibrary::~SharedLibrary() -{ - close(); - delete mPriv; -} - -bool SharedLibrary::open(const char *filename) -{ - mPriv->err_message.clear(); - close(); -#if defined(_WIN32) - mPriv->implementation = (void*)LoadLibrary(filename); - LPTSTR msg = nullptr; - FormatMessage( - FORMAT_MESSAGE_FROM_SYSTEM |FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_IGNORE_INSERTS, - nullptr, GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (LPTSTR)&msg, 0, nullptr); - - if(msg != nullptr) { - mPriv->err_message = std::string(msg); - // release memory allocated by FormatMessage() - LocalFree(msg); msg = nullptr; - } - return (mPriv->implementation != nullptr); -#else - mPriv->implementation = dlopen(filename, RTLD_LAZY); - char* msg = dlerror(); - if(msg) - mPriv->err_message = msg; - return mPriv->implementation != nullptr; -#endif -} - -bool SharedLibrary::close() { - int result = 0; - if (mPriv->implementation != nullptr) { -#if defined(WIN32) - result = FreeLibrary((HINSTANCE)mPriv->implementation); - LPTSTR msg = nullptr; - FormatMessage( - FORMAT_MESSAGE_FROM_SYSTEM |FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_IGNORE_INSERTS, - nullptr, GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (LPTSTR)&msg, 0, nullptr); - - if(msg != nullptr) { - mPriv->err_message = std::string(msg); - // release memory allocated by FormatMessage() - LocalFree(msg); msg = nullptr; - } -#else - result = dlclose(mPriv->implementation); - if (result != 0) { - char* msg = dlerror(); - if(msg) - mPriv->err_message = msg; - } -#endif - mPriv->implementation = nullptr; - - } - return (result == 0); -} - -std::string SharedLibrary::error() -{ - return mPriv->err_message; -} - -void *SharedLibrary::getSymbol(const char *symbolName) { - mPriv->err_message.clear(); - if (mPriv->implementation==nullptr) return nullptr; -#if defined(_WIN32) - FARPROC proc = GetProcAddress((HINSTANCE)mPriv->implementation, symbolName); - LPTSTR msg = nullptr; - FormatMessage( - FORMAT_MESSAGE_FROM_SYSTEM |FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_IGNORE_INSERTS, - nullptr, GetLastError(), MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT), - (LPTSTR)&msg, 0, nullptr); - - if(msg != nullptr) { - mPriv->err_message = std::string(msg); - // release memory allocated by FormatMessage() - LocalFree(msg); msg = nullptr; - } - return (void*)proc; -#else - dlerror(); - void* func = dlsym(mPriv->implementation,symbolName); - char* msg = dlerror(); - if(msg) - mPriv->err_message = msg; - return func; -#endif -} - -bool SharedLibrary::isValid() const { - return mPriv->implementation != nullptr; -} diff --git a/deps/sharedlibpp/src/SharedLibraryFactory.cpp b/deps/sharedlibpp/src/SharedLibraryFactory.cpp deleted file mode 100644 index ee38d48f3..000000000 --- a/deps/sharedlibpp/src/SharedLibraryFactory.cpp +++ /dev/null @@ -1,252 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include -#include -#include - -#include -#if defined(_WIN32) -# define shlibpp_struct_stat struct _stat -# define shlibpp_stat ::_stat -#else -# define shlibpp_struct_stat struct ::stat -# define shlibpp_stat ::stat -#endif - -class shlibpp::SharedLibraryFactory::Private -{ -public: - Private(int32_t startCheck, - int32_t endCheck, - int32_t systemVersion, - const char* factoryName); - - bool open(const char* dll_name); - bool isValid() const; - bool useFactoryFunction(void *factory); - - SharedLibrary lib; - SharedLibraryFactory::Status status; - SharedLibraryClassApi api; - int returnValue; - int rct; // FIXME Remove this reference counter and use a shared_ptr instead. - std::string name; - std::string className; - std::string baseClassName; - std::string error; - - int32_t startCheck; - int32_t endCheck; - int32_t systemVersion; - const char* factoryName; -}; - - - -shlibpp::SharedLibraryFactory::Private::Private(int32_t startCheck, - int32_t endCheck, - int32_t systemVersion, - const char* factoryName) : - status(Status::None), - returnValue(0), - rct(1), - startCheck(startCheck), - endCheck(endCheck), - systemVersion(systemVersion), - factoryName(factoryName) -{ - memset(&api, 0, sizeof(SharedLibraryClassApi)); -} - -bool shlibpp::SharedLibraryFactory::Private::open(const char* dll_name) -{ - returnValue = 0; - name = ""; - className = ""; - baseClassName = ""; - status = Status::None; - error = ""; - api.startCheck = 0; - if (!lib.open(dll_name)) { - shlibpp_struct_stat dummy; - if (shlibpp_stat(dll_name, &dummy) != 0) { - status = Status::LibraryNotFound; - } else { - status = Status::LibraryNotLoaded; - } - error = lib.error(); - return false; - } - void *fn = lib.getSymbol((factoryName != nullptr) ? factoryName : SHLIBPP_DEFAULT_FACTORY_NAME_STRING); - if (fn == nullptr) { - status = Status::FactoryNotFound; - error = lib.error(); - lib.close(); - return false; - } - if (!useFactoryFunction(fn)) { - status = Status::FactoryNotFunctional; - error = "Hook in shared library misbehaved"; - return false; - } - status = Status::OK; - name = dll_name; - - char buf[256]; - api.getClassName(buf, 256); - className = buf; - api.getBaseClassName(buf, 256); - baseClassName = buf; - - return true; -} - -bool shlibpp::SharedLibraryFactory::Private::isValid() const -{ - if (returnValue != startCheck) { - return false; - } - if (api.startCheck != startCheck) { - return false; - } - if (api.structureSize != sizeof(SharedLibraryClassApi)) { - return false; - } - if (api.systemVersion != systemVersion) { - return false; - } - if (api.endCheck != endCheck) { - return false; - } - return true; -} - -bool shlibpp::SharedLibraryFactory::Private::useFactoryFunction(void *factory) -{ - api.startCheck = 0; - if (factory == nullptr) { - return false; - } - returnValue = - ((int (*)(void *ptr,int len)) factory)(&api,sizeof(SharedLibraryClassApi)); - return isValid(); -} - - -shlibpp::SharedLibraryFactory::SharedLibraryFactory(int32_t startCheck, - int32_t endCheck, - int32_t systemVersion, - const char *factoryName) : - mPriv(new Private(startCheck, endCheck, systemVersion, factoryName)) -{ -} - -shlibpp::SharedLibraryFactory::SharedLibraryFactory(const char *dll_name, - int32_t startCheck, - int32_t endCheck, - int32_t systemVersion, - const char *factoryName) : - SharedLibraryFactory(startCheck, endCheck, systemVersion, factoryName) -{ - mPriv->open(dll_name); -} - -shlibpp::SharedLibraryFactory::SharedLibraryFactory(const char* dll_name, - const char* factoryName) : - SharedLibraryFactory(SHLIBPP_DEFAULT_START_CHECK, - SHLIBPP_DEFAULT_END_CHECK, - SHLIBPP_DEFAULT_SYSTEM_VERSION, - factoryName) -{ - mPriv->open(dll_name); -} - -shlibpp::SharedLibraryFactory::~SharedLibraryFactory() -{ - delete mPriv; -} - -bool shlibpp::SharedLibraryFactory::open(const char *dll_name, - int32_t startCheck, - int32_t endCheck, - int32_t systemVersion, - const char *factoryName) -{ - mPriv->startCheck = startCheck; - mPriv->endCheck = endCheck; - mPriv->systemVersion = systemVersion; - mPriv->factoryName = factoryName; - return mPriv->open(dll_name); -} - -bool shlibpp::SharedLibraryFactory::open(const char* dll_name, const char* factoryName) -{ - mPriv->startCheck = SHLIBPP_DEFAULT_START_CHECK; - mPriv->endCheck = SHLIBPP_DEFAULT_END_CHECK; - mPriv->systemVersion = SHLIBPP_DEFAULT_SYSTEM_VERSION; - mPriv->factoryName = factoryName; - return mPriv->open(dll_name); -} - -bool shlibpp::SharedLibraryFactory::isValid() const -{ - return mPriv->isValid(); -} - -shlibpp::SharedLibraryFactory::Status shlibpp::SharedLibraryFactory::getStatus() const -{ - return mPriv->status; -} - -std::string shlibpp::SharedLibraryFactory::getError() const -{ - return mPriv->error; -} - -const shlibpp::SharedLibraryClassApi& shlibpp::SharedLibraryFactory::getApi() const -{ - return mPriv->api; -} - -int shlibpp::SharedLibraryFactory::getReferenceCount() const -{ - return mPriv->rct; -} - -int shlibpp::SharedLibraryFactory::addRef() -{ - mPriv->rct++; - return mPriv->rct; -} - -int shlibpp::SharedLibraryFactory::removeRef() -{ - mPriv->rct--; - return mPriv->rct; -} - -std::string shlibpp::SharedLibraryFactory::getName() const -{ - return mPriv->name; -} - -std::string shlibpp::SharedLibraryFactory::getClassName() const -{ - return mPriv->className; -} - -std::string shlibpp::SharedLibraryFactory::getBaseClassName() const -{ - return mPriv->baseClassName; -} - -bool shlibpp::SharedLibraryFactory::useFactoryFunction(void *factory) -{ - return mPriv->useFactoryFunction(factory); -} diff --git a/deps/sharedlibpp/src/shlibpp/SharedLibrary.h b/deps/sharedlibpp/src/shlibpp/SharedLibrary.h deleted file mode 100644 index a044e143d..000000000 --- a/deps/sharedlibpp/src/shlibpp/SharedLibrary.h +++ /dev/null @@ -1,92 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_SHAREDLIBRARY_H -#define SHAREDLIBPP_SHAREDLIBRARY_H - -#include -#include - -namespace shlibpp { - -/** - * Low-level wrapper for loading shared libraries (DLLs) and accessing - * symbols within it. - */ -class SHLIBPP_API SharedLibrary -{ -public: - /** - * Initialize, without opening a shared library yet. - */ - SharedLibrary(); - - /** - * Load the named shared library / DLL. - * - * @param filename name of file (see open method) - */ - SharedLibrary(const char *filename); - - /** - * Destructor. Will close() if needed. - */ - virtual ~SharedLibrary(); - - SharedLibrary(const SharedLibrary& rhs) = delete; - SharedLibrary(SharedLibrary&& rhs) noexcept = delete; - SharedLibrary& operator=(const SharedLibrary& rhs) = delete; - SharedLibrary& operator=(SharedLibrary&& rhs) noexcept = delete; - - /** - * Load the named shared library / DLL. The library is found - * using the algoithm of ACE::ldfind. Operating-system-specific - * extensions will be tried, and the relevant path for shared - * libraries. - * - * @param filename name of file. - * @return true on success - */ - bool open(const char *filename); - - /** - * Shared library no longer needed, unload if not in use elsewhere. - * @return true on success - */ - bool close(); - - /** - * Returns a human-readable string describing the most recent error that - * occurred from a call to one of its functions. - * - * @return the most recent error - */ - std::string error(); - - /** - * Look up a symbol in the shared library. - */ - void *getSymbol(const char *symbolName); - - /** - * Check if the shared library is valid - * - * @return true iff a valid library has been loaded. - */ - bool isValid() const; - -#ifndef DOXYGEN_SHOULD_SKIP_THIS -private: - class SHLIBPP_HIDDEN Private; - Private* mPriv; -#endif -}; - -} // namespace shlibpp - -#endif // SHAREDLIBPP_SHAREDLIBRARY_H diff --git a/deps/sharedlibpp/src/shlibpp/SharedLibraryClass-inl.h b/deps/sharedlibpp/src/shlibpp/SharedLibraryClass-inl.h deleted file mode 100644 index 33ab54f95..000000000 --- a/deps/sharedlibpp/src/shlibpp/SharedLibraryClass-inl.h +++ /dev/null @@ -1,105 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_SHAREDLIBRARYCLASS_INL_H -#define SHAREDLIBPP_SHAREDLIBRARYCLASS_INL_H - - -template -shlibpp::SharedLibraryClass::SharedLibraryClass() : - content(nullptr), - pfactory(nullptr) -{ -} - -template -shlibpp::SharedLibraryClass::SharedLibraryClass(SharedLibraryClassFactory& factory) : - SharedLibraryClass() -{ - open(factory); -} - -template -shlibpp::SharedLibraryClass::~SharedLibraryClass() -{ - close(); -} - -template -bool shlibpp::SharedLibraryClass::open(SharedLibraryClassFactory& factory) -{ - close(); - content = factory.create(); - pfactory = &factory; - factory.addRef(); - - return content != nullptr; -} - -template -bool shlibpp::SharedLibraryClass::close() -{ - if (content != nullptr) { - pfactory->destroy(content); - if (pfactory->removeRef() == 0) { - delete pfactory; - } - } - - content = nullptr; - pfactory = nullptr; - - return true; -} - -template -T& shlibpp::SharedLibraryClass::getContent() -{ - return *content; -} - -template -const T& shlibpp::SharedLibraryClass::getContent() const -{ - return *content; -} - -template -bool shlibpp::SharedLibraryClass::isValid() const -{ - return content != nullptr; -} - - -template -T& shlibpp::SharedLibraryClass::operator*() -{ - return (*content); -} - - -template -const T& shlibpp::SharedLibraryClass::operator*() const -{ - return (*content); -} - - -template -T* shlibpp::SharedLibraryClass::operator->() -{ - return (content); -} - -template -const T* shlibpp::SharedLibraryClass::operator->() const -{ - return (content); -} - -#endif // SHAREDLIBPP_SHAREDLIBRARYCLASS_INL_H diff --git a/deps/sharedlibpp/src/shlibpp/SharedLibraryClass.h b/deps/sharedlibpp/src/shlibpp/SharedLibraryClass.h deleted file mode 100644 index 6a5ffe48d..000000000 --- a/deps/sharedlibpp/src/shlibpp/SharedLibraryClass.h +++ /dev/null @@ -1,128 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_SHAREDLIBRARYCLASS_H -#define SHAREDLIBPP_SHAREDLIBRARYCLASS_H - -#include - -namespace shlibpp { - -/** - * Container for an object created using a factory provided by a shared library. - * Used to ensure the object is destroyed by a method also provided by the - * shared library. Mixing creation and destruction methods could be very bad. - */ -template -class SharedLibraryClass -{ -public: - - /** - * Constructor for empty instance. - */ - SharedLibraryClass(); - - /** - * Constructor for valid instance of a class from a shared library. - * - * @param factory the factory to use to construct (and eventually - * destroy) the instance. - */ - SharedLibraryClass(SharedLibraryClassFactory& factory); - - /** - * Destructor. - */ - virtual ~SharedLibraryClass(); - - /** - * Construct an instance using the specified factory. If an - * instance has already been made, it is destroyed. - * - * @param factory the factory to use to construct (and eventually - * destroy) the instance. - * @return true on success - */ - bool open(SharedLibraryClassFactory& factory); - - /** - * Destroy an instance if one has been created. - * - * @return true on success - */ - virtual bool close(); - - /** - * Gives access to the created instance. - * - * No check made to ensure an instance is in fact present. - * Call SharedLibraryClass::isValid first if unsure. - * - * @return the created instance - */ - T& getContent(); - - /** - * Gives access to the created instance (const version). - * - * No check made to ensure an instance is in fact present. - * Call SharedLibraryClass::isValid first if unsure. - * - * @return the created instance - */ - const T& getContent() const; - - /** - * Check whether a valid instance has been created. - * - * @return true iff a valid instance has been created - */ - bool isValid() const; - - /** - * Shorthand for SharedLibraryClass::getContent - * - * @return the created instance - */ - T& operator*(); - - /** - * Shorthand for SharedLibraryClass::getContent (const version) - * - * @return the created instance - */ - const T& operator*() const; - - /** - * A pointer version of SharedLibraryClass::getContent - * - * @return a pointer to the created instance, or nullptr if there is none - */ - T* operator->(); - - /** - * A pointer version of SharedLibraryClass::getContent (const version) - * - * @return a pointer to the created instance, or nullptr if there is none - */ - const T* operator->() const; - -#ifndef DOXYGEN_SHOULD_SKIP_THIS -private: - T* content; - SharedLibraryClassFactory *pfactory; -#endif -}; - -} // namespace shlibpp - - -#include - -#endif // SHAREDLIBPP_SHAREDLIBRARYCLASS_H diff --git a/deps/sharedlibpp/src/shlibpp/SharedLibraryClassApi.h b/deps/sharedlibpp/src/shlibpp/SharedLibraryClassApi.h deleted file mode 100644 index 80b368b3f..000000000 --- a/deps/sharedlibpp/src/shlibpp/SharedLibraryClassApi.h +++ /dev/null @@ -1,195 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_SHAREDLIBRARYCLASSAPI_H -#define SHAREDLIBPP_SHAREDLIBRARYCLASSAPI_H - -#include -#include -#include - -namespace shlibpp { - -// Be careful loading C++ classes from DLLs. Generally you -// need an exact or very close match between compilers used -// to compile those DLLs and your own code. - -extern "C" { - - /** - * Collection of hooks for creating/destroying a plugin. - * Be careful to check carefully for compatibility before - * using create() or destroy(). - */ - struct SHLIBPP_API SharedLibraryClassApi { - public: - int32_t startCheck; // A 32-bit integer that is checked when loading - // a plugin. - // Don't touch anything further if it isn't. - int32_t structureSize; // size of the SharedLibraryClassApi. - // If this doesn't match what you expect, - // Don't touch anything further if it isn't. - int32_t systemVersion; // Overall version of plugin system. - // This does *not* cover compiler version etc. - - using createFn_t = void*(*)(); - using destroyFn_t = void(*)(void*); - using getFn_t = int32_t(*)(char*, size_t); - - createFn_t create; // Instantiate a plugin object. - destroyFn_t destroy; // Destroy a plugin object. - getFn_t getVersion; // Plugin-related version. - getFn_t getAbi; // Compiler-related version. - getFn_t getClassName; // Name of plugin (subclass). - getFn_t getBaseClassName; // Name superclass. - - int32_t roomToGrow[SHLIBPP_SHAREDLIBRARYCLASSAPI_PADDING]; // Padding. - int32_t endCheck; // A 32-bit integer that is checked when loading - // a plugin. - }; - -} // extern "C" - -} // namespace shlibpp - - -#define SHLIBPP_SHARED_CLASS_FN extern "C" SHLIBPP_EXPORT - -constexpr int32_t SHLIBPP_DEFAULT_START_CHECK = - static_cast('S') + - (static_cast('H') << 8) + - (static_cast('P') << 16) + - (static_cast('P') << 24); -constexpr int32_t SHLIBPP_DEFAULT_END_CHECK = - static_cast('P') + - (static_cast('L') << 8) + - (static_cast('U') << 16) + - (static_cast('G') << 24); -constexpr int32_t SHLIBPP_DEFAULT_SYSTEM_VERSION = 5; -#define SHLIBPP_DEFAULT_FACTORY_NAME shlibpp_default_factory - -#define SHLIBPP_STRINGIFY2(X) #X -#define SHLIBPP_STRINGIFY(X) SHLIBPP_STRINGIFY2(X) -#define SHLIBPP_DEFAULT_FACTORY_NAME_STRING SHLIBPP_STRINGIFY(SHLIBPP_DEFAULT_FACTORY_NAME) - - -/** - * Macro to create a bunch of functions with undecorated names that can - * be found within a plugin library to handle creation/deletion of that - * plugin. Use with care. - * - * @param factoryname the name of the "hook" function to make. - * A collection of other helper functions with names composed of the - * factoryname with _create/_destroy/... appended. - * @param startcheck a 32-bit integer that is checked when loading a plugin. - * @param endcheck a 32-bit integer that is checked when loading a plugin. - * @param systemversiona a 32-bit integer representing the version of the plugin - * api that is checked when loading a plugin. - * @param classname the class that the hook will be able to instantiate. - * @param basename the superclass that the user of the plugin should be - * working with. - */ -#define SHLIBPP_DEFINE_SHARED_SUBCLASS_CUSTOM(startcheck, endcheck, systemversion, factoryname, classname, basename) \ - SHLIBPP_SHARED_CLASS_FN void* factoryname ## _create () \ - { \ - classname* cn = new classname; \ - basename* bn = dynamic_cast(cn); \ - if (!bn) { \ - delete cn; \ - } \ - return static_cast(bn); \ - } \ - \ - SHLIBPP_SHARED_CLASS_FN void factoryname ## _destroy (void* obj) \ - { \ - classname* cn = dynamic_cast(static_cast(obj)); \ - if(cn) { \ - delete cn; \ - } \ - } \ - \ - SHLIBPP_SHARED_CLASS_FN int32_t factoryname ## _getVersion (char* ver, size_t len) \ - { \ - return 0; \ - } \ - \ - SHLIBPP_SHARED_CLASS_FN int32_t factoryname ## _getAbi (char* abi, size_t len) \ - { \ - return 0; \ - } \ - \ - SHLIBPP_SHARED_CLASS_FN int32_t factoryname ## _getClassName (char* name, size_t len) \ - { \ - char cname[] = # classname; \ - strncpy(name, cname, len); \ - return strlen(cname) + 1; \ - } \ - \ - SHLIBPP_SHARED_CLASS_FN int32_t factoryname ## _getBaseClassName (char* name, size_t len) \ - { \ - char cname[] = # basename; \ - strncpy(name, cname, len); \ - return strlen(cname) + 1; \ - } \ - \ - SHLIBPP_SHARED_CLASS_FN int32_t factoryname(void* api, size_t len) { \ - struct shlibpp::SharedLibraryClassApi* sapi = static_cast(api); \ - if (len < sizeof(shlibpp::SharedLibraryClassApi)) { \ - return -1; \ - } \ - sapi->startCheck = startcheck; \ - sapi->structureSize = sizeof(shlibpp::SharedLibraryClassApi); \ - sapi->systemVersion = systemversion; \ - sapi->create = factoryname ## _create; \ - sapi->destroy = factoryname ## _destroy; \ - sapi->getVersion = factoryname ## _getVersion; \ - sapi->getAbi = factoryname ## _getAbi; \ - sapi->getClassName = factoryname ## _getClassName; \ - sapi->getBaseClassName = factoryname ## _getBaseClassName; \ - for (int i=0; iroomToGrow[i] = 0; \ - } \ - sapi->endCheck = endcheck; \ - return sapi->startCheck; \ - } -// The double cast in the _create() and _destroy() functions are -// required to ensure that everything works when `basename` is not the -// first inherited class: -// _create() will return a valid `basename` or a null pointer if -// `classname` does not inherit from `basename`. -// _destroy() will ensure that we are calling `classname` destructor -// even if `basename` is not the first inherited class. If the -// dynamic_cast fails, it will not delete the object (that is probably -// leaked), but it is less dangerous than executing some other random -// function. - -#define SHLIBPP_DEFINE_SHARED_SUBCLASS(factoryname, classname, basename) \ - SHLIBPP_DEFINE_SHARED_SUBCLASS_CUSTOM(SHLIBPP_DEFAULT_START_CHECK, \ - SHLIBPP_DEFAULT_END_CHECK, \ - SHLIBPP_DEFAULT_SYSTEM_VERSION, \ - factoryname, \ - classname, \ - basename) - -#define SHLIBPP_DEFINE_DEFAULT_SHARED_CLASS(classname) \ - SHLIBPP_DEFINE_SHARED_SUBCLASS_CUSTOM(SHLIBPP_DEFAULT_START_CHECK, \ - SHLIBPP_DEFAULT_END_CHECK, \ - SHLIBPP_DEFAULT_SYSTEM_VERSION, \ - SHLIBPP_DEFAULT_FACTORY_NAME, \ - classname, \ - classname) - -#define SHLIBPP_DEFINE_SHARED_CLASS(factoryname, classname) \ - SHLIBPP_DEFINE_SHARED_SUBCLASS_CUSTOM(SHLIBPP_DEFAULT_START_CHECK, \ - SHLIBPP_DEFAULT_END_CHECK, \ - SHLIBPP_DEFAULT_SYSTEM_VERSION, \ - factoryname, \ - classname, \ - classname) - -#endif // SHAREDLIBPP_SHAREDLIBRARYCLASSAPI_H diff --git a/deps/sharedlibpp/src/shlibpp/SharedLibraryClassFactory-inl.h b/deps/sharedlibpp/src/shlibpp/SharedLibraryClassFactory-inl.h deleted file mode 100644 index f126f0599..000000000 --- a/deps/sharedlibpp/src/shlibpp/SharedLibraryClassFactory-inl.h +++ /dev/null @@ -1,56 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_SHAREDLIBRARYCLASSFACTORY_INL_H -#define SHAREDLIBPP_SHAREDLIBRARYCLASSFACTORY_INL_H - -template -shlibpp::SharedLibraryClassFactory::SharedLibraryClassFactory(int32_t startCheck, - int32_t endCheck, - int32_t systemVersion, - const char *factoryName) : - SharedLibraryFactory(startCheck, endCheck, systemVersion, factoryName) -{ -} - -template -shlibpp::SharedLibraryClassFactory::SharedLibraryClassFactory(const char *dll_name, - int32_t startCheck, - int32_t endCheck, - int32_t systemVersion, - const char *factoryName) : - SharedLibraryFactory(dll_name, startCheck, endCheck, systemVersion, factoryName) -{ -} - -template -shlibpp::SharedLibraryClassFactory::SharedLibraryClassFactory(const char *dll_name, - const char *factoryName) : - SharedLibraryFactory(dll_name, factoryName) -{ -} - -template -T* shlibpp::SharedLibraryClassFactory::create() const -{ - if (!isValid()) { - return nullptr; - } - return static_cast(getApi().create()); -} - -template -void shlibpp::SharedLibraryClassFactory::destroy(T *obj) const -{ - if (!isValid()) { - return; - } - getApi().destroy(obj); -} - -#endif // SHAREDLIBPP_SHAREDLIBRARYCLASSFACTORY_INL_H diff --git a/deps/sharedlibpp/src/shlibpp/SharedLibraryClassFactory.h b/deps/sharedlibpp/src/shlibpp/SharedLibraryClassFactory.h deleted file mode 100644 index e0826a208..000000000 --- a/deps/sharedlibpp/src/shlibpp/SharedLibraryClassFactory.h +++ /dev/null @@ -1,51 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_SHAREDLIBRARYCLASSFACTORY_H -#define SHAREDLIBPP_SHAREDLIBRARYCLASSFACTORY_H - -#include -#include - -namespace shlibpp { - -/** - * A type-safe wrapper for SharedLibraryFactory, committing to - * creation/destruction of instances of a particular super-class. - * - * Note that we take on faith that the named factory method in the - * named shared library does in fact create the named type. - */ -template -class SharedLibraryClassFactory : public SharedLibraryFactory -{ -public: - explicit SharedLibraryClassFactory(int32_t startCheck = SHLIBPP_DEFAULT_START_CHECK, - int32_t endCheck = SHLIBPP_DEFAULT_END_CHECK, - int32_t systemVersion = SHLIBPP_DEFAULT_SYSTEM_VERSION, - const char *factoryName = nullptr); - - explicit SharedLibraryClassFactory(const char *dll_name, - int32_t startCheck = SHLIBPP_DEFAULT_START_CHECK, - int32_t endCheck = SHLIBPP_DEFAULT_END_CHECK, - int32_t systemVersion = SHLIBPP_DEFAULT_SYSTEM_VERSION, - const char *factoryName = nullptr); - - explicit SharedLibraryClassFactory(const char *dll_name, - const char *factoryName = nullptr); - - T *create() const; - - void destroy(T *obj) const; -}; - -} // namespace shlibpp - -#include - -#endif // SHAREDLIBPP_SHAREDLIBRARYCLASSFACTORY_H diff --git a/deps/sharedlibpp/src/shlibpp/SharedLibraryFactory.h b/deps/sharedlibpp/src/shlibpp/SharedLibraryFactory.h deleted file mode 100644 index 6efeb4dab..000000000 --- a/deps/sharedlibpp/src/shlibpp/SharedLibraryFactory.h +++ /dev/null @@ -1,235 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_SHAREDLIBRARYFACTORY_H -#define SHAREDLIBPP_SHAREDLIBRARYFACTORY_H - -#include -#include - -namespace shlibpp { - -class SharedLibraryClassApi; - -/** - * A wrapper for a named factory method in a named shared library. - * This wrapper will do some basic checks that the named method does - * indeed behave like a shlibpp plugin hook before offering access to it. - * This is to avoid accidents, it is not a security mechanism. - */ -class SHLIBPP_API SharedLibraryFactory -{ -public: - /** - * The status of a factory can be: - * - None: Not configured yet - * - OK: Present and sane - * - LibraryNotFound: Named shared library was not found - * - LibraryNotLoaded: Named shared library failed to load - * - FactoryNotFound: Named method wasn't present in library - * - FactoryNotFunctional: Named method is not working right - */ - enum class Status : std::uint32_t - { - None = 0, //!< Not configured yet. - OK, //!< Present and sane. - LibraryNotFound, //!< Named shared library was not found. - LibraryNotLoaded, //!< Named shared library failed to load. - FactoryNotFound, //!< Named method wasn't present in library. - FactoryNotFunctional //!< Named method is not working right. - }; - - /** - * Constructor for unconfigured factory with custom start check, end check, - * system version and factory name. - * - * @param startCheck a 32-bit integer that is checked when loading a plugin. - * It must be the same used when creating the plugin - * @param endCheck a 32-bit integer that is checked when loading a plugin. - * It must be the same used when creating the plugin - * @param systemVersion a number representing the version of the plugin api - * that is checked when loading a plugin. - * It must be the same used when creating the plugin. - * @param factoryName name of factory method, a symbol within the shared - * library. - * If set, it must be the same used when creating the - * plugin. - */ - explicit SharedLibraryFactory(int32_t startCheck = -1, - int32_t endCheck = -1, - int32_t systemVersion = -1, - const char *factoryName = nullptr); - - /** - * Constructor with custom start check, end check, system version and - * factoryName - * - * @param dll_name name/path of shared library. - * @param startCheck a 32-bit integer that is checked when loading a plugin. - * It must be the same used when creating the plugin - * @param endCheck a 32-bit integer that is checked when loading a plugin. - * It must be the same used when creating the plugin - * @param systemVersion a number representing the version of the plugin api - * that is checked when loading a plugin. - * It must be the same used when creating the plugin. - * @param factoryName name of factory method, a symbol within the shared - * library. - * If set, it must be the same used when creating the - * plugin. - */ - SharedLibraryFactory(const char *dll_name, - int32_t startCheck = -1, - int32_t endCheck = -1, - int32_t systemVersion = -1, - const char *factoryName = nullptr); - - /** - * Constructor with default start check, end check and system version. - * - * @param dll_name name/path of shared library. - * @param factoryName name of factory method, a symbol within the shared - * library. - * If set, it must be the same used when creating the - * plugin. - */ - SharedLibraryFactory(const char *dll_name, - const char *factoryName = nullptr); - - /** - * Destructor - */ - virtual ~SharedLibraryFactory(); - - /** - * Configure the factory. - * - * @param dll_name name/path of shared library. - * @param startCheck a 32-bit integer that is checked when loading a plugin. - * It must be the same used when creating the plugin - * @param endCheck a 32-bit integer that is checked when loading a plugin. - * It must be the same used when creating the plugin - * @param systemVersion a number representing the version of the plugin api - * that is checked when loading a plugin. - * It must be the same used when creating the plugin. - * @param factoryName name of factory method, a symbol within the shared - * library. - * If set, it must be the same used when creating the - * plugin. - * @return true on success. - */ - bool open(const char *dll_name, - int32_t startCheck = -1, - int32_t endCheck = -1, - int32_t systemVersion = -1, - const char *factoryName = nullptr); - - /** - * Configure the factory. - * - * @param dll_name name/path of shared library. - * @param factoryName name of factory method, a symbol within the shared - * library. - * If set, it must be the same used when creating the - * plugin. - * @return true on success. - */ - bool open(const char *dll_name, - const char *factoryName = nullptr); - - /** - * Check if factory is configured and present. - * - * @return true iff factory is good to go. - */ - bool isValid() const; - - /** - * Get the status of the factory. - * - * @return one of the SharedLibraryFactory::STATUS_* codes. - */ - Status getStatus() const; - - /** - * Get the latest error of the factory. - * - * @return the latest error. - */ - std::string getError() const; - - /** - - * Get the factory API, which has creation/deletion methods. - * - * @return the factory API - */ - const SharedLibraryClassApi& getApi() const; - - /** - * Get the current reference count of this factory. - * - * @return the current reference count of this factory. - */ - int getReferenceCount() const; - - /** - * Increment the reference count of this factory. - * - * @return the current reference count of this factory, after increment. - */ - int addRef(); - - /** - * Decrement the reference count of this factory. - * - * @return the current reference count of this factory, after decrement. - */ - int removeRef(); - - /** - * Get the name associated with this factory. - * - * @return the name associated with this factory. - */ - std::string getName() const; - - /** - * Get the type associated with this factory. - * - * @return the type associated with this factory. - */ - std::string getClassName() const; - - /** - * Get the base type associated with this factory. - * - * @return the base type associated with this factory. - */ - std::string getBaseClassName() const; - - /** - * - * Specify function to use as factory. - * - * @param factory function to use as factory. - * - * @result true on success. - * - */ - bool useFactoryFunction(void *factory); - -#ifndef DOXYGEN_SHOULD_SKIP_THIS -private: - class SHLIBPP_HIDDEN Private; - Private* mPriv; -#endif -}; - -} // namespace shlibpp - -#endif // SHAREDLIBPP_SHAREDLIBRARYFACTORY_H diff --git a/deps/sharedlibpp/src/shlibpp/api.h b/deps/sharedlibpp/src/shlibpp/api.h deleted file mode 100644 index 7416c207c..000000000 --- a/deps/sharedlibpp/src/shlibpp/api.h +++ /dev/null @@ -1,44 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_API_H -#define SHAREDLIBPP_API_H - -#if defined _WIN32 || defined __CYGWIN__ -# define SHLIBPP_HELPER_DLL_EXPORT __declspec(dllexport) -# define SHLIBPP_HELPER_DLL_IMPORT __declspec(dllimport) -# define SHLIBPP_HELPER_DLL_LOCAL -#else -# if __GNUC__ >= 4 -# define SHLIBPP_HELPER_DLL_EXPORT __attribute__ ((visibility("default"))) -# define SHLIBPP_HELPER_DLL_IMPORT __attribute__ ((visibility("default"))) -# define SHLIBPP_HELPER_DLL_LOCAL __attribute__ ((visibility("hidden"))) -# else -# define SHLIBPP_HELPER_DLL_EXPORT -# define SHLIBPP_HELPER_DLL_IMPORT -# define SHLIBPP_HELPER_DLL_LOCAL -# endif -#endif - -#define SHLIBPP_EXPORT SHLIBPP_HELPER_DLL_EXPORT -#define SHLIBPP_IMPORT SHLIBPP_HELPER_DLL_IMPORT -#define SHLIBPP_LOCAL SHLIBPP_HELPER_DLL_LOCAL - -#ifdef SHLIBPP_STATIC -# define SHLIBPP_API -# define SHLIBPP_HIDDEN -#else -# ifdef BUILDING_SHLIBPP -# define SHLIBPP_API SHLIBPP_HELPER_DLL_EXPORT -# else -# define SHLIBPP_API SHLIBPP_HELPER_DLL_IMPORT -# endif -# define SHLIBPP_HIDDEN SHLIBPP_HELPER_DLL_LOCAL -#endif - -#endif // SHAREDLIBPP_API_H diff --git a/deps/sharedlibpp/src/shlibpp/config.h.in b/deps/sharedlibpp/src/shlibpp/config.h.in deleted file mode 100644 index 2dd197d45..000000000 --- a/deps/sharedlibpp/src/shlibpp/config.h.in +++ /dev/null @@ -1,15 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_CONFIG_H -#define SHAREDLIBPP_CONFIG_H - -#define SHLIBPP_POINTER_SIZE @CMAKE_SIZEOF_VOID_P@ -#define SHLIBPP_SHAREDLIBRARYCLASSAPI_PADDING (30-2*(SHLIBPP_POINTER_SIZE/4)) - -#endif // SHAREDLIBPP_CONFIG_H diff --git a/deps/sharedlibpp/src/shlibpp/version.h.in b/deps/sharedlibpp/src/shlibpp/version.h.in deleted file mode 100644 index 0249e766f..000000000 --- a/deps/sharedlibpp/src/shlibpp/version.h.in +++ /dev/null @@ -1,29 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#ifndef SHAREDLIBPP_VERSION_H -#define SHAREDLIBPP_VERSION_H - -#include -#include - -#define SHLIBPP_VERSION_MAJOR @shlibpp_VERSION_MAJOR@ -#define SHLIBPP_VERSION_MINOR @shlibpp_VERSION_MINOR@ -#define SHLIBPP_VERSION_PATCH @shlibpp_VERSION_PATCH@ -#define SHLIBPP_VERSION "@shlibpp_VERSION@" - -namespace shlibpp { - -SHLIBPP_API int getVersionMajor(); -SHLIBPP_API int getVersionMinor(); -SHLIBPP_API int getVersionPatch(); -SHLIBPP_API std::string getVersion(); - -} // namespace shlibpp - -#endif // SHAREDLIBPP_VERSION_H diff --git a/deps/sharedlibpp/src/version.cpp b/deps/sharedlibpp/src/version.cpp deleted file mode 100644 index 935d2877a..000000000 --- a/deps/sharedlibpp/src/version.cpp +++ /dev/null @@ -1,29 +0,0 @@ -/* - * Copyright (C) 2006-2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * BSD-3-Clause license. See the accompanying LICENSE file for details. - */ - -#include - -int shlibpp::getVersionMajor() -{ - return SHLIBPP_VERSION_MAJOR; -} - -int shlibpp::getVersionMinor() -{ - return SHLIBPP_VERSION_MINOR; -} - -int shlibpp::getVersionPatch() -{ - return SHLIBPP_VERSION_PATCH; -} - -std::string shlibpp::getVersion() -{ - return SHLIBPP_VERSION; -} diff --git a/deps/thrift/CMakeLists.txt b/deps/thrift/CMakeLists.txt index 223070d1c..aa127bbdb 100644 --- a/deps/thrift/CMakeLists.txt +++ b/deps/thrift/CMakeLists.txt @@ -38,7 +38,7 @@ target_include_directories(ClockRpc PUBLIC install( TARGETS ClockRpc - EXPORT WBToolboxLibraryExport + EXPORT WBToolboxExport LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) diff --git a/doc/doxygen/index.md b/doc/doxygen/index.md index e77019b31..198cda0b1 100644 --- a/doc/doxygen/index.md +++ b/doc/doxygen/index.md @@ -2,20 +2,20 @@ ### A Simulink Toolbox for Whole Body Control -> This toolbox allows non-programming experts and researchers approaching _Whole Body Control_ to more easily develop controllers on either simulated or real `YARP`-based robotic platforms.
+> This toolbox allows non-programming experts and researchers approaching _Whole-Body Control_ to more easily develop controllers on either simulated or real `YARP`-based robotic platforms.
> Develop to deployment time is minimized by exploiting the rich variety of Simulink's toolboxes and its capabilities on rapid prototyping and visual debugging. -Visit the [WB-Toolbox Website](https://robotology.github.io/wb-toolbox/) for more informations. +`WBT` is based on the dataflow framework [`blockfactory`](https://github.com/robotology/blockfactory). Visit the [Whole-Body Toolbox Website](https://robotology.github.io/wb-toolbox/) for more information. -## Who use `WB-Toolbox` +## Who uses `WBT` -`WB-Toolbox` is used extensively in the controllers stored in [robotology-playground/whole-body-controllers](https://github.com/robotology-playground/whole-body-controllers). +`WBT` is used extensively in the controllers stored in [robotology/whole-body-controllers](https://github.com/robotology/whole-body-controllers). -[This video](https://youtu.be/UXU3KSa201o) shows the latest results on the iCub robot achieved in the EU project CoDyCo in which a top level controller implemented with the `WB-Toolbox` achieves a running `100 Hz` rate. +[This video](https://youtu.be/UXU3KSa201o) shows the latest results on the iCub robot achieved in the EU project CoDyCo in which a top level controller implemented with the `WBT` achieves a running `100 Hz` rate. ### Citing this work -Please cite the following publication if you are using WB-Toolbox for your own research and/or robot controllers: +Please cite the following publication if you are using Whole-Body Toolbox for your own research and/or robot controllers: > Romano F., Traversaro S., Pucci D., Nori F.
> **A Whole-Body Software Abstraction layer for Control Design of free-floating Mechanical Systems**
@@ -32,7 +32,7 @@ Bibtex citation: ### Acknowledgments -The development of WB-Toolbox is supported by: +The development of Whole-Body Toolbox is supported by: - FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics) - H2020 EU projects AnDy (No. 731540 H2020-ICT-2016-1) diff --git a/doc/mkdocs/data/about.md b/doc/mkdocs/data/about.md index 003879562..aa05c9231 100644 --- a/doc/mkdocs/data/about.md +++ b/doc/mkdocs/data/about.md @@ -1,8 +1,8 @@ -`WB-Toolbox` has been developed in the [Dynamic Interaction Control](https://www.iit.it/research/lines/dynamic-interaction-control) research line at the [Italian Institute of Technology](https://www.iit.it). +`WBT` has been developed in the [Dynamic Interaction Control](https://dic.iit.it) research line at the [Italian Institute of Technology](https://www.iit.it). It was born from the ashes of @robotology-playground/WBI-Toolbox. -It is used extensively for controllers stored in @robotology-playground/whole-body-controllers. +It is used extensively for controllers stored in @robotology/whole-body-controllers. ### Maintainer @@ -21,7 +21,7 @@ In alphabetical order: ## Acknowledgments -The development of `WB-Toolbox` is supported by: +The development of `WBT` is supported by: - FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics) - H2020 EU projects AnDy (No. 731540 H2020-ICT-2016-1) diff --git a/doc/mkdocs/data/create_new_block.md b/doc/mkdocs/data/create_new_block.md deleted file mode 100644 index f84f498b4..000000000 --- a/doc/mkdocs/data/create_new_block.md +++ /dev/null @@ -1,76 +0,0 @@ -!!! warning - These information are outdated. The need to be ported to `WB-Toolbox 3`. - -The following steps are necessary in order to add additional blocks to the Library. - -## C++ - -### Generic Block -- Inherit from `Block` class -- Implement the `numberOfParameters()` function returning the number of parameters your block takes -- If the parameter is tunable (i.e. it can be changed during the simulation) implement `parameterAtIndexIsTunable`: by default parameters are not tunable -- Implement the `configureSizeAndPorts` method to properly set the number and type of input and output ports -- Implement the `initialize` method to perform any initialization your block requires -- Implement the `finilize` method to cleanup any resources your block requested in the `initialize` method -- Implement the `output`method to perform the actual operations - -You can access the block parameters by using the usual mex C functions. **NOTE** your parameters are 1-based numbered, **NOT** 0-based. So the first parameter is at index 1, etc... - -### WBI-based Block -If you need to implement a WBI-based block it is highly advisable that you inherit from `WBIBlock` class. -This base class already implements a lot of functionalities and it is highly probable you need to just implement the `output` function. - -The `WBIBlock` base class already provide you the following features: - -- The number of parameters is already set to `4` (`robot name`, `model name`, `wbi filename`, `wbi joint list`) and they are correctly parsed. -- An instance of the `Yarp WholeBody interface` is configured, initialized, and properly released in the `finilize` method. -- The Yarp network properly initialized and terminated. -- You can obtain a reference to the singleton WB Interface wrapper by calling the static method `WBInterface::sharedInstance`. - -**Note** Additional parameters you specify starts from the index `5`. - -### Notes on implementation - -- During `configureSizeAndPorts` you should not allocate any memory or save any data because the object will not persist after the method call. The correct place is the `initialize` method. -- Every function takes as last parameter an `Error` object. It can be `NULL`, so check before dereferencing the pointer. - -## Final steps - -Independently of the type of block you implemented some more steps are required to properly add the block. - -### CMake - -You can use the macro provided by this project: - -```cmake -configure_block(BLOCK_NAME ${HUMAN_READABLE_DESCRIPTION} - LIST_PREFIX WBT - SOURCES ${CPP_FILES} - HEADERS ${HEADER_FILES}) -``` - -where - - - `${HUMAN_READABLE_DESCRIPTION}` is a string used in the group folder (for projects which support it), - - `${CPP_FILES}` is a list of `.cpp` files needed by your block - - `${HEADER_FILES}` is a list of `.h` files needed by your block - -### Block Factory - -- Add you main header to the `toolbox.h` file -- Add the code needed for the creation of your class in `factory.cpp`, `Block::instantiateBlockWithClassName` method. The string passed as argument is the one you specify in the S-Function block in Simulink (see next section) - -## Simulink - -- Add an S-Function block -- Specify as s-function name `WB-Toolbox` -- Add the parameters: - - The first parameter is the name of the class, e.g. `YarpRead` - - If you are creating a WBI-based block you have to specify 4 additional parameters: `robot name`, `model name`, `YarpWBI configuration file`, `YarpWBI Joint list` - - Add any additional parameter required by your block -- Create a Subsystem and add a Mask to it - -**Note** you should use variables for the S-Function parameters and you should specify them in the mask parameters. -**Note** you can also supply default values for the variables by adding code in the `Block->Properties->Callbacks->Init` - -You can skip most of the aforementioned steps by duplicating an already existing block. Just be sure you are duplicating it and not creating an alias. diff --git a/doc/mkdocs/data/create_new_library.md b/doc/mkdocs/data/create_new_library.md deleted file mode 100644 index 6f3e1bdcb..000000000 --- a/doc/mkdocs/data/create_new_library.md +++ /dev/null @@ -1,647 +0,0 @@ -# Tutorial: How to create a new library - -This guide describes how to use the core infrastructure of the Whole Body Toolbox for creating a new toolbox **Toolbox Example**. It will contain a single block _Signal math_ with the following specifications: - -- Accepts two input signals -- Performs element-wise operations: sum, subtraction, multiplication -- Allows to select the operation with a user friendly GUI (mask) -- Produces an output signal with the result of the operation - -Despite it is a very trivial example, it shows how to structure both the C++ and the Matlab components of a toolbox. From this starting point is then possible to build more complex architectures which might need e.g. to be split in many parts or to link against third-party libraries. - -!!! example "Toolbox Example project" - You can find the files of this example in the [`example`](https://github.com/robotology/wb-toolbox/tree/master/example) folder. - -!!! info - Until `v4` this core machinery is shipped together with our robotics framework. We're currently working on splitting the toolbox components for providing a standalone process. Stay tuned for upcoming news! - -## Introduction - -Before jumping in the example of this tutorial, in this section you will find helpful information useful to grasp the key ideas about the toolbox and a refreshing of common terms and patterns used in programming. - -### Algorithm specifications - -The execution of a generic algorithm can be split in the following basic phases: - -1. Configuration -2. Initialization -3. Step -4. Termination - -In the configuration phase the algorithm can, for instance, read parameters and specify details about its inputs and outputs. During the initialization it might need to allocate resources. When everything is ready, the simulation starts and on every cycle of the loop the algorithm computes a step. Before finishing the simulation, in the termination step the resources that are not anymore needed can be deallocated and final operations can be executed. - -### Terminology - -There are few key components which are part of the core infrastructure, and it is important they are clear from the very beginning. - -!!! note - This project has strong roots with Simulink. Despite it is not anymore strictly related to it, the structure keeps as inheritance its terminology. - -#### Block - -The Block is the main component of the toolbox. You may think of it as a wrapper for a generic algorithm. It provides support of all the phases discussed above. - -#### Port - -Blocks need to interface with the outside for reading input data and writing their results. Ports are attached to the Block and let data flow in and out from it. They have properties like the accepted data type and they are characterized by a size. - -#### Signal - -A Signal is the generalization of a buffer. It can be plugged to multiple Ports and allows sharing data between them. Similarly to Ports, a Signal has a data type and a width. When a Signal is connected to a Port, their dimension must match. - -#### Engine - -The engine is the framework that calls and executes the code implementing the Blocks. We currently provide support for Simulink and Simulink Coder. Alternative engines might be Scilab or even standalone C++ code. - -#### BlockInformation - -BlockInformation is the interface between a Block and the engine that executes its code. Despite blocks are not aware of what engine is executing them, blocks and engine need to exchange information such as user parameters and Port data. BlockInformation provides methods for setting and getting this piece of information. - -#### Simulink Block - -A Simulink Block is the wrapper of a (C++) Block. It provides a visual representation of it, with input and output ports and gives the possibility to connect it with other Simulink blocks through signals. The terms above come from this representation and map it to C++. - -#### Simulink Model - -A Simulink Model is a project composed of a set of Simulink Blocks interconnected by Signals. - -#### Simulink Library - -A Simulink Library is a container of a set of Blocks that populates the _Simulink Library Browser_. Every toolbox must be shipped with an associated Simulink Library file in order to expose its Blocks. - -#### S-Function - -There are many types of functions for implementing an algorithm wrapped by a Simulink Block. In Matlab terminology, these functions are referred as [S-Functions](https://it.mathworks.com/help/simulink/sfg/what-is-an-s-function.html). You can read more about the supported types in [What type of S-Function should you use](https://it.mathworks.com/help/simulink/sfg/what-type-of-s-function-should-you-use.html). - -In short S-Functions provide a sort of interface (through C callbacks) where a given number of functions need to be implemented. Simulink knows the name of these functions and calls them on demand. - -#### Block Mask - -A Simulink Block is just a square with input and output ports. It is possible to "mask" a Simulink Block in order to provide a user-friendly GUI that can be useful e.g. for setting Block parameters. The mask may contain buttons, sliders, check boxes, dropdown menus, etc. - -#### Software library - -A library is a file containing compiled code (functions, classes, etc.) which cannot be executed standalone. It can be either _static_ or _dynamic_. Static libraries are meant to be embedded inside the executable that calls their symbols, instead the code of dynamic libraries (also called shared libraries) is only referenced inside the executable and called when needed. - -For grasping better this difference, if multiple executables link against the same static library, the same code is embedded inside all of them, resulting in bigger executables and code duplication. A dynamic library object instead can be shared by multiple executables that need only to know their location in the filesystem and which symbols they provide. - -!!! info - You can find more detailed information about software libraries and linkers at [this link](https://www.lurklurk.org/linkers/linkers.html). - -#### Plugin Library - -A plugin library is a particular type of a dynamic library. An executable can load dynamic libraries either at load time, i.e. when the executable is launched, or at run time, i.e. when needed during the execution. The libraries that can be loaded during run time are referred as plugins. - -!!! info - On UNIX systems the load the a plugin is executed by a `dlopen` - -### Overview of Simulink execution - -The core of the WB-Toolbox provides a framework capable of loading during runtime shared libraries. When the Simulink simulation starts, its engine assigns a deterministic order to the blocks of the Simulink Model. If one of these blocks is not contained in the system toolboxes, it needs to be associated to a _S-Function_ that implements its logic. The toolbox provides a streamlined approach to implement these functions without the need of being an expert of the complex Simulink APIs. - -The blocks of our library are implemented in Simulink using the following _S-Function_ block: - -![EmptySFunctionBlock](images/EmptySFunctionBlock.png) - -What you need to know for the time being is that the name of the plugin library generated from the C++ code will be stored in the _S-function parameters_ field. We provide a generic **Level-2 MEX S-Function** that reads this parameter and uses the library name name to dynamically load the dynamic shared library that contains the block's code. - -After the library is found in the filesystem and successfully loaded, Simulink allocates an object of the C++ class associated to the block functionality. Again, this information (the class name) is passed in the _S-function parameters_ field. - -## Develop the C++ plugin - -You already learned that Blocks are no more than regular C++ classes. They are not an exception, in fact all the components discussed until now are mapped to C++ classes or interfaces ([abstract classes](https://en.cppreference.com/w/cpp/language/abstract_class)). - -The following resources provide further information about them: - -- Blocks are implementations of the [`wbt::Block`](https://robotology.github.io/wb-toolbox/doxygen/classwbt_1_1_block.html) interface -- BlockInformation is defined in the [`wbt::BlockInformation`](https://robotology.github.io/wb-toolbox/doxygen/classwbt_1_1_block_information.html) interface -- Signals are mapped to the [`wbt::Signal`](https://robotology.github.io/wb-toolbox/doxygen/classwbt_1_1_signal.html) class -- Blocks parameters are mapped to the [`wbt::Parameter`](https://robotology.github.io/wb-toolbox/doxygen/classwbt_1_1_parameter.html) class - -We need the following folder structure for the C++ project. Create already the empty files so the project can compile from the very first attempt. - -``` -. -├── CMakeLists.txt -├── include -│   └── SignalMath.h -└── src - ├── Factory.cpp - └── SignalMath.cpp -``` - -!!! info - All the path of this tutorial will be relative to the root folder of the project. This means that if the directory tree above is stored in `/home/foo/tutorial`, when you read to go the `./build` directory it means `/home/foo/tutorial/build`. - -!!! tip - Bear in mind that this C++ class is independent from Simulink. Potentially, it can be called by a standalone C++ executable. - -!!! tip - It is not mandatory to implement a new class for every Simulink Block. If they share a lot of code, a single class can be referenced by multiple Simulink Blocks and its behavior can be selected using parameters. - -### CMake project for compiling the library - -You are free to use your favorite tool for compiling the project. We recommend CMake. If you are not an expert user of this tool, just follow the steps. The comments in the file should be enough to understand what it is happening. - -Fill the file `CMakeLists.txt` with the following content: - -```cmake -cmake_minimum_required(VERSION 3.5) -project(ExampleToolbox LANGUAGES CXX VERSION 0.1) - -# C++ standard -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -# Export all symbols in Windows -set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) - -# Tweak linker flags in Linux. -# Matlab is very strict on missing symbols and by default ld do not warn if -# something is missing. -if(UNIX AND NOT APPLE) - get_filename_component(LINKER_BIN ${CMAKE_LINKER} NAME) - if(${LINKER_BIN} STREQUAL "ld") - set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--unresolved-symbols=report-all") - endif() -endif() - -# =========== -# C++ LIBRARY -# =========== - -# Find the needed WBToolbox components: -# - ToolboxCore contains the core classes such as Block and Signal -# - ToolboxMex is required at runtime for loading the library from Simulink -find_package(WBToolbox 4 REQUIRED COMPONENTS ToolboxCore ToolboxMex) - -# Find the project for the multiplatform support of plugin libraries. -# It is shipped with WBToolbox and it should be already installed in your system. -find_package(shlibpp REQUIRED) - -# Create the plugin library. This must be a SHARED library. -add_library(ExampleToolbox SHARED - include/SignalMath.h - src/SignalMath.cpp - src/Factory.cpp) - -# Manually set the name of the output library. This is not required and it -# is done only for sake of clarity. -set_target_properties(ExampleToolbox PROPERTIES - OUTPUT_NAME "ExampleToolbox") - -# Link the library with the Core component containing the core classes -# and the target that provides the plugin support -target_link_libraries(ExampleToolbox - WBToolbox::Core shlibpp::shlibpp) - -# Setup the include directories -target_include_directories(ExampleToolbox PUBLIC - $ - $) -``` - -!!! note - If your library needs to link against other libraries, use `find_package` to load their targets and then add them to the `target_link_libraries` directive. - -From the root folder of the project, execute: - -```bash -mkdir build -cd build -cmake .. -cmake --build . -``` - -You should now find in the `./build` directories a new library file, which depending on your OS is: - -- `libExampleToolbox.so` on Linux -- `libExampleToolbox.dylib` on macOS -- `ExampleToolbox.dll` on Windows - -This is the toolbox's plugin library which is loaded during runtime by the Engine. - -### Implement the block logic - -The only _Signal math_ block of our new toolbox will be implemented in a `example::SignalMath` C++ class. - -#### Header - -Here below the `./include/SignalMath.h` header. - -Given the simple logic it should be straightforward to understand. The class inherits from the `wbt::Block` interface and implements some of its methods. - -!!! info - The only mandatory method to implement is the `wbt::Block::output`. By default the other methods are dummy and they always return `true`. - -```cpp -#ifndef EXAMPLE_SIGNALMATH_H -#define EXAMPLE_SIGNALMATH_H - -#include -#include - -#include -#include - -namespace example { - class SignalMath; -} // namespace example - -class example::SignalMath : public wbt::Block -{ -private: - enum class Operation - { - ADDITION, - SUBTRACTION, - MULTIPLICATION, - }; - - Operation m_operation; - -public: - static const std::string ClassName; - - SignalMath() = default; - ~SignalMath() override = default; - - unsigned numberOfParameters() override; - bool parseParameters(wbt::BlockInformation* blockInfo) override; - bool configureSizeAndPorts(wbt::BlockInformation* blockInfo) override; - bool initialize(wbt::BlockInformation* blockInfo) override; - bool output(const wbt::BlockInformation* blockInfo) override; - bool terminate(const wbt::BlockInformation* blockInfo) override; -}; - -#endif // EXAMPLE_SIGNALMATH_H - -``` - -The `./src/SignalMath.cpp` file is more interesting, let's split it in chunks. - -#### Parameters - -If you recall, BlockInformation is used by the Block for interfacing with the Engine. When this code is executed inside Simulink, the [`wbt::SimulinkBlockInformation`](https://robotology.github.io/wb-toolbox/doxygen/classwbt_1_1_simulink_block_information.html) implementation will perform the required operations calling Simulink APIs. - -```cpp -#include "SignalMath.h" - -#include -#include -#include - -using namespace example; - -unsigned SignalMath::numberOfParameters() -{ - // The base wbt::Block class needs parameters (e.g. the ClassName). - // You must specify here how many more parameters this class needs. - // Our example needs just one more: the operation to perform. - return Block::numberOfParameters() + 1; -} - -// This method should let BlockInformation know the parameters metadata -bool SignalMath::parseParameters(wbt::BlockInformation* blockInfo) -{ - // Initialize information for our only parameter - int rows = 1; - int cols = 1; - std::string name = "Operation"; // This label is used to access the param later - unsigned index = Block::numberOfParameters(); // Indices start from 0 - wbt::ParameterType type = wbt::ParameterType::STRING; - - // Create the parameter - wbt::ParameterMetadata parameterMetadata(type, index, rows, cols, name); - - // Add the parameter metadata into the BlockInformation - if (!blockInfo->addParameterMetadata(parameterMetadata)) { - wbtError << "Failed to store parameter metadata"; - return false; - } - - // Ask to the BlockInformation interface to parse the parameters and store them into - // the m_parameters variable. This variable is contained in the wbt::Block class. - bool paramParsedOk = blockInfo->parseParameters(m_parameters); - - // Return the outcome of the parameter parsing. - // If the parsing fails, the execution stops. - return paramParsedOk; -} -``` - -#### Configuration - -The configuration of the Block is performed in the following steps: - -1. The base class needs to be configured. It needs some parameters (e.g. the class name and the library name) and this call asks the Engine to parse them. -2. The ports of the Block need to be defined. In this example the size is set as dynamic so that it accepts signals with any width. -3. The [`wbt::BlockInformation::IOData`](https://robotology.github.io/wb-toolbox/doxygen/structwbt_1_1_block_information_1_1_i_o_data.html) class is used to store the data of all the ports. It is a `struct` containing two `std::vectors`. -4. The data is then sent to the Engine through the BlockInformation interface. - -!!! info - If needed, parameters can be accessed from this step. Refer to the initialization phase to understand how to gather them. - -!!! warning "Signal size" - Simulink has the support of inheriting the port size from the signal size, though use this feature only when strictly needed. In complex Simulink Models it might be difficult executing this size propagation, and fixing the Port size provides helpful constraints for the Engine. - -!!! danger "Important" - Be careful on memory allocations during this step. A temporary object is created only for configuration means, and then destroyed. All the allocated memory will be hereby deleted. - -```cpp -bool SignalMath::configureSizeAndPorts(wbt::BlockInformation* blockInfo) -{ - // The base wbt::Block class need to be configured - if (!wbt::Block::configureSizeAndPorts(blockInfo)) { - return false; - } - - // Create data about input and output ports. - wbt::BlockInformation::PortData input1; - wbt::BlockInformation::PortData input2; - wbt::BlockInformation::PortData output; - input1 = {/*portIndex=*/0, std::vector{wbt::Signal::DynamicSize}, wbt::DataType::DOUBLE}; - input2 = {/*portIndex=*/1, std::vector{wbt::Signal::DynamicSize}, wbt::DataType::DOUBLE}; - output = {/*portIndex=*/0, std::vector{wbt::Signal::DynamicSize}, wbt::DataType::DOUBLE}; - - // Populate a structure with the overall input / output data - wbt::BlockInformation::IOData ioData; - ioData.input.push_back(input1); - ioData.input.push_back(input2); - ioData.output.push_back(output); - - // Store this data into the BlockInformation - if (!blockInfo->setIOPortsData(ioData)) { - wbtError << "Failed to configure input / output ports"; - return false; - } - - return true; -} -``` - -#### Initialization - -!!! info - Starting from this step, memory persistence is guaranteed. - -In the initialization step, the input parameter that defines the operation is parsed. In this example the parameter is passed as a string. In the header a new `enum class Operation` was defined and here the related private member is properly initialized. Additional checks can be added, i.e. testing that both ports have the same width given that a dynamic size was previously assigned. - -!!! info - In this case there's no need to allocate memory. If your class has buffers to initialize, this is the right time to do it. - -```cpp -bool SignalMath::initialize(wbt::BlockInformation* blockInfo) -{ - // The base wbt::Block class need to be initialized - if (!Block::initialize(blockInfo)) { - return false; - } - - // Parse the parameters - if (!SignalMath::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; - return false; - } - - // Read the Operation parameter and store it as a private member - std::string operation; - if (!m_parameters.getParameter("Operation", operation)) { - wbtError << "Failed to parse Operation parameter"; - return false; - } - - // Check the content of the parameter - if (operation == "Addition") { - m_operation = Operation::ADDITION; - } - else if (operation == "Subtraction") { - m_operation = Operation::SUBTRACTION; - } - else if (operation == "Multiplication") { - m_operation = Operation::MULTIPLICATION; - } - else { - wbtError << "Operation " << operation << " not supported"; - return false; - } - - // Check that the size of the input signals match - if (blockInfo->getInputPortWidth(/*index=*/0) != blockInfo->getInputPortWidth(/*index=*/1)) { - wbtError << "Input signals widths do not match"; - return false; - } - - return true; -} -``` - -#### Output - -The `output` method is where the real algorithm is implemented. The Signals are firstly gathered from the Engine using their index. The classes `wbt::InputSignalPtr` and `wbt::OutputSignalPtr` are particular `typedef` of the [`wbt::Signal`](https://robotology.github.io/wb-toolbox/doxygen/classwbt_1_1_signal.html) class and they have the same methods. In the `for` loop the configured operation is performed and the result stored in the output signal. - -!!! note - Note that input signals are read-only. You can write data only to the output signals objects. - -```cpp -bool SignalMath::output(const wbt::BlockInformation* blockInfo) -{ - // Get the input signals - wbt::InputSignalPtr input1 = blockInfo->getInputPortSignal(/*index=*/0); - wbt::InputSignalPtr input2 = blockInfo->getInputPortSignal(/*index=*/1); - - // Get the output signal - wbt::OutputSignalPtr output = blockInfo->getOutputPortSignal(/*index=*/0); - - // Check the signal validity - if (!input1 || !input2 || !output) { - wbtError << "Signals not valid"; - return false; - } - - // Check the width of the output signal. - // This is recommended for dynamically sized signals. - if (output->getWidth() != input1->getWidth()) { - wbtError << "Output signal has a width of " << output->getWidth() - << " while input signals have a width of " << input1->getWidth(); - return false; - } - - // Perform the given operation - for (unsigned i = 0; i < output->getWidth(); ++i) { - switch (m_operation) { - case Operation::ADDITION: - output->set(i, input1->get(i) + input2->get(i)); - break; - case Operation::SUBTRACTION: - output->set(i, input1->get(i) - input2->get(i)); - break; - case Operation::MULTIPLICATION: - output->set(i, input1->get(i) * input2->get(i)); - break; - } - } - - return true; -} -``` - -#### Terminate - -Given the simplicity of our Block, the `terminate` step is a dummy implementation. This method is reported just for the sake of clarity. It can be omitted since `wbt::Block::terminate` already provides the same dummy implementation. - -```cpp -bool SignalMath::terminate(const wbt::BlockInformation* /*blockInfo*/) -{ - return true; -} -``` - -### Implement the plugin factory - -A plugin library usually contains multiple classes used for multiple Blocks. The `sharedlibpp` tool for plugins requires declaring what classes are part of the plugin. This operation is done in the `./include/Factory.cpp` file: - -```cpp -#include "SignalMath.h" - -// Class factory API -#include - -// Add the example::SignalMath class to the plugin factory -SHLIBPP_DEFINE_SHARED_SUBCLASS(SignalMath, example::SignalMath, wbt::Block); -``` - -The `SHLIBPP_DEFINE_SHARED_SUBCLASS` macro needs the following three arguments: - -- A label used to extract the class from the plugin library -- The class of the block -- The base class of the block - -The only interesting part here is the label. Keep this name in mind because we need to know it later in the Simulink section. - -If everything works as expected, the library is now ready and can be compiled again: - -```bash -cd build -cmake --build . -``` - -!!! tip - Simulink will open this library at the beginning of the simulation loop and it needs to find it from the filesystem. Be sure that the `./build` folder is in the searching path of your dynamic linker. In the supported OSs you should add it to: - - **Linux** `LD_LIBRARY_PATH` - - **macOS** `DYLIB_LIBRARY_PATH` - - **Windows** `Path` - -## Matlab and Simulink - -Once the C++ library is ready, the classes can be wrapped by a Simulink Block. If, as in this case, there's no existing Simulink Library to which the new block can be added, some extra step to create a new one is necessary. - -We're going to store the files discussed in this section in the `./matlab` folder, obtaining at the end the following project structure: - -``` -. -├── CMakeLists.txt -├── include -│   └── SignalMath.h -├── matlab -│   ├── ExampleToolbox.slx -│   ├── Model.mdl -│   └── slblocks.m -└── src -    ├── Factory.cpp -    └── SignalMath.cpp -``` - -- `ExampleToolbox.slx` is the Simulink Library -- `slblocks.m` is a m script necessary for loading external Simulink Libraries into the Simulink Library Browser -- `Model.mdl` is a Simulink Model for testing the Block - -### Create an new Simulink Library - -The first step is creating a new Simulink Library. Open the Simulink Start Page and create a Blank Library. Save it in `./matlab/ExampleToolbox.slx`. - -![NewBlankLibrary](images/NewBlankLibrary.png) - -In order to [populate the Simulink Library Browser](https://it.mathworks.com/help/simulink/ug/adding-libraries-to-the-library-browser.html), you need to create a `slblocks.m` file with the following content: - -```matlab -function blkStruct = slblocks - -Browser.Library = 'ExampleToolbox'; -Browser.Name = 'Example Toolbox'; -Browser.IsFlat = 0; - -blkStruct.Browser = Browser; -``` - -As explained in the [official documentation](https://it.mathworks.com/help/simulink/ug/adding-libraries-to-the-library-browser.html), we also need to modify a property of the library file: - -```matlab ->> set_param('ExampleToolbox','EnableLBRepository','on'); -``` - -If you followed these steps, the new library should be ready. Be sure that the `./matlab` folder is in the Matlab `path`, then open the Simulink Library Browser and press ++f5++. You should now see the empty _Example Toolbox_ entry. - -!!! note - In order to add a folder to the `path`, you can either browse it from the Matlab tree view making it the current folder, or executing `addpath('/path/of/the/folder')`. Find more details at [What Is the MATLAB Search Path?](https://it.mathworks.com/help/matlab/matlab_env/what-is-the-matlab-search-path.html). - -!!! info - Every time you close the library, Matlab locks it. Press the lock icon in the bottom left corner to unlock the library. - -### Add a block - -Open the library and add a new _S-Function_ block with the following parameters: - -![SFunctionBlock](images/SFunctionBlock.png) - -- The _S-function name_ is the name of the generic S-Function that Whole Body Toolbox provides -- The _S-function parameters_ are the parameters passed to the S-Function. The first two are required by `wbt::Block`, and the third one is the parameter of our `example::SignalMath` class: - - `'SignalMath'` is the label we used in `Factory.cpp` - - `'ExampleToolbox'` is the name of the plugin library. It must match the `OUTPUT_NAME` we assigned in `CMakeLists.txt` - - `'Addition'` is the kind of operation we want this block to perform - -Press OK and save the library. If you insert wrong information, like a wrong number of parameters or parameters with a wrong type or content, the `wbtError`s we added in the C++ class should provide more details. - -### Create a test Simulink Model - -Now it's time for finally testing all our work. Create a new _Blank Model_ and populate it with the following blocks: - -![TestSimulinkModel](images/TestSimulinkModel.png) - -Then press the Play icon, and the _Display_ connected to the block output should show the result of the addition. - -### Create a block mask - -The type of the operation is defined as a parameter of the _S-Function_ block. It is not very intuitive changing it in this way. Beyond the effort of changing the string, we should remember exactly what parameters the block accepts. - -This limitation can be overcome masking the block, that means providing a GUI to the block. Right-click the block from the _Example Toolbox_ library and press `Mask > Create Mask`. - -In the `Icon & Ports` tab, fill the `Icon drawing commands` with: - -```matlab -disp('Signal Math') - -port_label('input',1,'Input 1') -port_label('input',2,'Input 2') - -port_label('output',1,'Output') -``` - -Then, in the `Parameters & Dialog` tab, add a `Popup` and fill it obtaining the following status: - -![BlockMask](images/BlockMask.png) - -!!! note - Note that the evaluate attribute has been unchecked. When a Popup is not evaluated, its variable (`operation` in this case) will contain the string of the selected option. Instead, if it is evaluated, it contains a 0-based index. - -Save the library and substitute the new Block in the Simulink Model. You can now select the operation double-clicking the block and changing the popup menu entry. - -![TestSimulinkModelWithMask](images/TestSimulinkModelWithMask.png) - -## Final comments - -Whole Body Toolbox provides great abstraction capabilities for wrapping C and C++ algorithms to Simulink Blocks. You don't need to be a Simulink expert for this kind of operation, all the machinery is hidden under the hood. - -Possibilities are endless. Despite Whole Body Toolbox was originally designed for robotic applications, it is interesting discovering how many new toolboxes can be created on top of it. The `v4` release provides the first experimental support to external usage. The toolbox APIs are already mature and reached an acceptable stability after being developed, tested, and refined for many years. - -We are eager to hear your feedback in order to have a great `v1.0` release of the standalone toolbox core! If you find bugs or want to propose enhancements, please fill a [new Issue](https://github.com/robotology/wb-toolbox/issues?q=is%3Aissue+is%3Aopen+sort%3Aupdated-desc). diff --git a/doc/mkdocs/data/getting-started.md b/doc/mkdocs/data/getting-started.md deleted file mode 100644 index 5bf6e655b..000000000 --- a/doc/mkdocs/data/getting-started.md +++ /dev/null @@ -1,18 +0,0 @@ -## Environment variables - -The following environment variables must be specified: - -- `YARP_ROBOT_NAME` -- `YARP_DATA_DIRS` - -!!! tip "Tip: launch Matlab from the command line" - The environment variables stored in the `.bashrc` or `.bash_profile` files are automatically loaded. Store here additional variables if needed. - -!!! tip "Tip: launch Matlab from the desktop launcher" - You can store environment variables from the Matlab command line using the `setenv` function. - -## 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. - -In order to start dragging and dropping blocks from the `WB-Toolbox`, open the _Simulink Library Browser_ and search for `Whole Body Toolbox` in the tree view. diff --git a/doc/mkdocs/data/index.md b/doc/mkdocs/data/index.md index 667edec72..85cd0bf09 100644 --- a/doc/mkdocs/data/index.md +++ b/doc/mkdocs/data/index.md @@ -1,21 +1,26 @@ -# Whole Body Toolbox +# Whole-Body Toolbox +## A Simulink Toolbox for Whole-Body Control -## A Simulink Toolbox for Whole Body Control +[![Build Status (master)](https://img.shields.io/travis/robotology/wb-toolbox/master.svg?logo=travis&label=master)](https://travis-ci.org/robotology/wb-toolbox) +[![Build Status (devel)](https://img.shields.io/travis/robotology/wb-toolbox/devel.svg?logo=travis&label=devel)](https://travis-ci.org/robotology/wb-toolbox) +[![Codacy Badge](https://api.codacy.com/project/badge/Grade/1c726331d58b4a1ebfba1c25d15f00ad)](https://www.codacy.com/app/diegoferigo/wb-toolbox?utm_source=github.com&utm_medium=referral&utm_content=robotology/wb-toolbox&utm_campaign=Badge_Grade) -!!! quote "" - This toolbox allows non-programming experts and researchers approaching _Whole Body Control_ to more easily develop controllers on either simulated or real `YARP`-based robotic platforms.
- Develop to deployment time is minimized by exploiting the rich variety of Simulink's toolboxes and its capabilities on rapid prototyping and visual debugging. - -![](http://drive.google.com/uc?export=view&id=0B6zDGh11iY6oc0gtM0lMdDNweWM) +[![License](https://img.shields.io/badge/license-LGPL-19c2d8.svg)](https://github.com/robotology/wb-toolbox/blob/master/LICENSE.LGPL2) +[![GitHub release](https://img.shields.io/github/release/robotology/wb-toolbox.svg)](https://github.com/robotology/wb-toolbox/releases) +[![GitHub issues](https://img.shields.io/github/issues-raw/robotology/wb-toolbox.svg)](https://github.com/robotology/wb-toolbox/issues) + -[![GitPitch](https://gitpitch.com/assets/badge.svg)](https://gitpitch.com/robotology/WB-Toolbox?grs=github) +!!! quote "" +​ This toolbox allows non-programming experts and researchers approaching _Whole-Body Control_ to more easily develop controllers on either simulated or real `YARP`-based robotic platforms.
+​ Develop to deployment time is minimized by exploiting the rich variety of Simulink's toolboxes and its capabilities on rapid prototyping and visual debugging. +`WBT` is based on the dataflow framework [`blockfactory`](https://github.com/robotology/blockfactory). It mainly wraps functionalities of the [`YARP`](https://github.com/robotology/yarp) middleware and the [`iDynTree`](https://github.com/robotology/idyntree) rigid-body dynamics library, providing an interface compatible with dataflow programming. The `WBT` library can be embedded in any C++ framework and run from all the engines supported by [`blockfactory`](https://github.com/robotology/blockfactory), we mainly use and support Simulink by providing a Simulink Library. -## Who use `WB-Toolbox` +## Who uses `WBT`? -`WB-Toolbox` is used extensively in the controllers stored in @robotology-playground/whole-body-controllers. +`WBT` is used extensively in the controllers stored in @robotology/whole-body-controllers. -This video shows the latest results on the iCub robot achieved in the EU project CoDyCo in which a top level controller implemented with the @robotology/WB-Toolbox achieves a running `100 Hz` rate. +This video shows the latest results on the iCub robot achieved in the EU project CoDyCo in which a top level controller implemented with the @robotology/wb-toolbox achieves a running `100 Hz` rate. [![IMAGE ALT TEXT](http://img.youtube.com/vi/VrPBSSQEr3A/0.jpg)](https://youtu.be/UXU3KSa201o "iCub balancing on one foot via external force control and interacting with humans") diff --git a/doc/mkdocs/data/install.md b/doc/mkdocs/data/install.md index ca8c1546d..e3cd307e9 100644 --- a/doc/mkdocs/data/install.md +++ b/doc/mkdocs/data/install.md @@ -1,15 +1,17 @@ # Install !!! info "Disclaimer" - `WB-Toolbox` has been widely tested on `Ubuntu 16:04` and `Ubuntu 18.04` with Matlab `R2017b`. If you face any issue either with your OS or Matlab version, please submit an [Issue](https://github.com/robotology/WB-Toolbox/issues). + `WBT` has been widely tested on `Ubuntu 16:04` and `Ubuntu 18.04` with Matlab `R2017b`. If you face any issue either with your OS or Matlab version, please submit an [Issue](https://github.com/robotology/wb-toolbox/issues). ## Requirements -- Matlab 7.1+ and Simulink: tested with Matlab `R2017b`, `R2016b` +- [`blockfactory`](https://github.com/robotology/blockfactory) - [`YARP`](https://github.com/robotology/yarp) compiled as shared library (default behavior) - [`iDynTree`](https://github.com/robotology/idyntree) - [`YCM`](https://github.com/robotology/ycm) -- Supported Operating Systems: Linux, macOS, Windows +- Supported Operating Systems: Linux, macOS, Windows + +### Development requirements ## Optional requirements @@ -17,50 +19,73 @@ - [Gazebo Simulator](http://gazebosim.org/) - [`gazebo_yarp_plugins`](https://github.com/robotology/gazebo_yarp_plugins) - [`qpOASES`](https://github.com/robotology-dependencies/qpOASES/) +- [`matio`](https://github.com/tbeu/matio) ## Installation For a simplified installation procedure, jump to [Install using the `robotology-superbuild`](#install-using-the-robotology-superbuild). - !!! warning The following instructions are for Unix-like systems, but they work similarly on other operating systems. ### Dependencies -Install the required and the optional dependencies by following their installation instructions. These instructions need that `YARP`, `iDynTree` and `YCM` packages can be found by `CMake` using `find_package`. +Install the required and the optional dependencies by following their installation instructions. These instructions need that `blockfactory`, `YARP`, `iDynTree` and `YCM` packages can be found by `CMake` using `find_package`. !!! warning - If an optional dependency is not found, the classes depending on it are not compiled. However, in the Simulink Library the block does not disappear. It will just not work, raising an appropriate error. + If an optional dependency is not found, the classes depending on it are not compiled. However, in the Simulink Library the blocks do not disappear. They will just not work, raising an appropriate error. ### Setup Matlab -Make sure that you have MATLAB and Simulink properly installed and running. +Matlab and Simulink are not required to build the `WBT` plugin. However, through [`blockfactory`](https://github.com/robotology/blockfactory) the plugin library shipped in this repository can be loaded inside a Simulink model. `WBT` provides a Simulink Library that exposes all the C++ blocks of the plugin to Simulink, wrapping them in user-friendly masks. + +!!! info + Despite the blocks can be used directly from C++, this usage is not very user friendly. Simulink and alternative visual tools are very convenient solutions to connect blocks together, but nothing prevents using the provided blocks exploiting the `blockfactory` interfaces. From now on, we assume that you want to use Simulink since it is the most common use case. -`CMake` needs to find the Matlab installation folder in order to link the sources against its libraries. Make sure that `CMake` is able to [find your Matlab installation](https://cmake.org/cmake/help/v3.3/module/FindMatlab.html), or manually set the `Matlab_ROOT_DIR` environment variable if needed. +#### For developers -After this, check that the MEX compiler for MATLAB is properly configured and working. 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). +Developers of `WBT` must have Simulink installed in order to operate on the provided Simulink Library. + +Make sure that `CMake` is able to [find your Matlab installation](https://cmake.org/cmake/help/v3.3/module/FindMatlab.html), or manually set the `Matlab_ROOT_DIR` environment variable if needed. ### Download, build and install If all the dependencies are met, proceed with the following instructions: -```sh -git clone https://github.com/robotology/wb-toolbox.git -mkdir -p wb-toolbox/build && cd wb-toolbox/build -cmake .. -DCMAKE_INSTALL_PREFIX= -cmake --build . --config Release -cmake --build . --config Release --target install -``` +!!! example "Commands" + + Substitute to `` the absolute path where you want to install the project. -!!! note - From now on, this guide refers to your install directory with the variable ``. Every time you see this variable, you should substitute the absolute install path. + ````tab="GNU / Linux and macOS" + git clone https://github.com/robotology/wb-toolbox.git + mkdir -p wb-toolbox/build && cd wb-toolbox/build + cmake .. -DCMAKE_INSTALL_PREFIX= + cmake --build . + cmake --build . --target install + ```` + + ````tab="Windows" + git clone https://github.com/robotology/wb-toolbox.git + mkdir -p wb-toolbox/build && cd wb-toolbox/build + cmake .. -DCMAKE_INSTALL_PREFIX= + cmake --build . --config Release + cmake --build . --config Release --target install + ```` ## Configuration +### Plugin + +In order to use Whole-Body Toolbox, the dynamic loader of the operating system should be able to find the plugin library. Add the folder `/lib` to the following environment variable depending on the OS: + +| GNU / Linux | macOS | Windows | +| ----------------- | ------------------- | ------- | +| `LD_LIBRARY_PATH` | `DYLD_LIBRARY_PATH` | `Path` | + + ### Matlab -In order to use the `WB-Toolbox` in Matlab you have to add some folders to the Matlab path. +In order to use the `WBT` in Matlab you have to add some folders to the Matlab path. If you usually launch Matlab from the command line, exporting the following environment variable should be enough: @@ -75,16 +100,16 @@ addpath(['' /mex]) addpath(genpath(['' /share/WBToolbox])) ``` -We also provide for the latter scenario a persistent configuration of `WB-Toolbox`. After the installation run once the `startup_wbitoolbox.m` script that you can find in the `/share/WBToolbox` directory. It will place a file `pathdef.m` in your `userpath` that loads the right variables to Matlab's environment. Note that this usage assumes that Matlab is always launched from the [`userpath` folder](https://it.mathworks.com/help/matlab/matlab_env/assign-userpath-as-the-startup-folder-on-unix-or-macintosh.html). +We also provide for the latter scenario a persistent configuration of `WBT`. After the installation, run once the `startup_wbitoolbox.m` script that you can find in the `/share/WBToolbox` directory. It will place a file `pathdef.m` in your `userpath` that loads the right variables to Matlab's environment. Note that this usage assumes that Matlab is always launched from the [`userpath` folder](https://it.mathworks.com/help/matlab/matlab_env/assign-userpath-as-the-startup-folder-on-unix-or-macintosh.html). ### Environment -Each robot that can be used through `WB-Toolbox` has its own configuration files. `WB-Toolbox` uses the `YARP`'s [`ResourceFinder`](http://www.yarp.it/yarp_resource_finder_tutorials.html) for finding files in the file system. You should thus follow the related instructions to properly configure your installation (e.g. setting the `YARP_DATA_DIRS` and `YARP_ROBOT_NAME` variables). +Each robot that can be used with `WBT` has its own configuration files. `WBT` uses the `YARP`'s [`ResourceFinder`](http://www.yarp.it/yarp_resource_finder_tutorials.html) for finding files in the file system. You should thus follow the related instructions to properly configure your installation (e.g. setting the `YARP_DATA_DIRS` and `YARP_ROBOT_NAME` variables). ## Install using the `robotology-superbuild` The @robotology/robotology-superbuild provides an easy way for users to setup an environment by downloading, compiling, installing all the projects together. -Follow the [superbuild installation instructions](https://github.com/robotology/robotology-superbuild/#installation) and enable the `ROBOTOLOGY_ENABLE_DYNAMICS` profile. If `WB-Toolbox` is not downloaded and built, check that the `ROBOTOLOGY_USES_MATLAB` is `ON` and the `ROBOTOLOGY_NOT_USE_SIMULINK` is `OFF`. +Follow the [superbuild installation instructions](https://github.com/robotology/robotology-superbuild/#installation) and enable the `ROBOTOLOGY_ENABLE_DYNAMICS` profile. If `WBT` is not downloaded and built, check that `ROBOTOLOGY_USES_MATLAB` is `ON` and `ROBOTOLOGY_NOT_USE_SIMULINK` is `OFF`. -The configuration should be straightforward following the [Configure your environment](https://github.com/robotology/robotology-superbuild/#configure-your-environment) and [Matlab](https://github.com/robotology/robotology-superbuild#matlab) sections. +The configuration of the environment should be straightforward following the [Configure your environment](https://github.com/robotology/robotology-superbuild/#configure-your-environment) and [Matlab](https://github.com/robotology/robotology-superbuild#matlab) sections. diff --git a/doc/mkdocs/data/license.md b/doc/mkdocs/data/license.md index 5b0d3f650..fa0281c1b 100644 --- a/doc/mkdocs/data/license.md +++ b/doc/mkdocs/data/license.md @@ -1,6 +1,6 @@ ### LGPL v2.1 or any later version -Copyright (C) 2018 Istituto Italiano di Tecnologia - iCub Facility +Copyright (C) 2019 Istituto Italiano di Tecnologia - iCub Facility This library is free software; you can redistribute it and/or modify it under the terms of the GNU Lesser General Public diff --git a/doc/mkdocs/data/sim_tricks.md b/doc/mkdocs/data/sim_tricks.md deleted file mode 100644 index 52b09a061..000000000 --- a/doc/mkdocs/data/sim_tricks.md +++ /dev/null @@ -1,31 +0,0 @@ -!!! warning - These information have not been updated for some while and might be outdated. - -This page contains some tricks (or not so obvious features) needed for creating Blocks in simulink - -### Use of popup mask parameters -If you need to use a popup mask parameter (i.e. a dropdown list) in you block you should notice the following thing: - -- If **EVALUATE** is set to **TRUE** then the variable associated with the parameter will contain the **INDEX** (1-based) of the selection. -- If **EVALUATE** is set to **FALSE** then the variable associated with the parameter will contain the **ACTUAL VALUE** of the selection. - -[Original source](http://www.mathworks.com/matlabcentral/answers/95199-how-can-i-assign-the-actual-value-of-a-selected-popup-to-my-mask-variables-in-simulink-6-6-r2007a) - -### Self-modifiable blocks -If you block needs to modify its options (via matlab callbacks associated with a parameter), e.g. when changing the parameters mask depending on another parameter, or when changing the number of input/outputs, be sure to check (on) the "Mask Editor -> Initialization -> Allow library block to modify its contents" - -![](https://cloud.githubusercontent.com/assets/2189180/11606884/19fd2f5a-9b32-11e5-9f1e-89347a6df895.png) - -### Create a new block -This is more heuristic than the "correct method", but it has been proved to work. - -Create a new Simulink Model and start creating the block. You can also duplicate an existing block from the Library. In this second case be sure to do the following steps: - -- Right click on the block -> Library Link -> Disable Link -- Right click on the block -> Library Link -> Break Link - -At this point finish your work, compile the model and only at this point add the block to the library, so that its aspect will reflect the real one. - -#### Note: -The step of creating the model separately is needed until we find out how to update the aspect of the block directly in the Library. -Further more, this process can hinder the already existing links to the block. diff --git a/doc/mkdocs/data/simulink_basic.md b/doc/mkdocs/data/simulink_basic.md new file mode 100644 index 000000000..49efb36d8 --- /dev/null +++ b/doc/mkdocs/data/simulink_basic.md @@ -0,0 +1,2 @@ +!!! warning + TODO diff --git a/doc/mkdocs/data/simulink_gazebo.md b/doc/mkdocs/data/simulink_gazebo.md new file mode 100644 index 000000000..49efb36d8 --- /dev/null +++ b/doc/mkdocs/data/simulink_gazebo.md @@ -0,0 +1,2 @@ +!!! warning + TODO diff --git a/doc/mkdocs/data/simulink_model_blocks.md b/doc/mkdocs/data/simulink_model_blocks.md new file mode 100644 index 000000000..49efb36d8 --- /dev/null +++ b/doc/mkdocs/data/simulink_model_blocks.md @@ -0,0 +1,2 @@ +!!! warning + TODO diff --git a/doc/mkdocs/data/troubleshooting.md b/doc/mkdocs/data/troubleshooting.md index 3fdfb0e11..a860446bb 100644 --- a/doc/mkdocs/data/troubleshooting.md +++ b/doc/mkdocs/data/troubleshooting.md @@ -16,25 +16,3 @@ The current version on Ubuntu 16:04 is `libstdc++.so.6`, make sure this is the c !!! info Another solution involving the `.matlab7rc.sh` file can be found in https://github.com/robotology/codyco-superbuild/issues/141#issuecomment-257892256. - -## `YARP` not installed in the system default directory - -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 - -```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 -``` diff --git a/doc/mkdocs/data/tutorial_introduction.md b/doc/mkdocs/data/tutorial_introduction.md new file mode 100644 index 000000000..49efb36d8 --- /dev/null +++ b/doc/mkdocs/data/tutorial_introduction.md @@ -0,0 +1,2 @@ +!!! warning + TODO diff --git a/doc/mkdocs/mkdocs.yml b/doc/mkdocs/mkdocs.yml index 626597539..68b9c112d 100644 --- a/doc/mkdocs/mkdocs.yml +++ b/doc/mkdocs/mkdocs.yml @@ -1,6 +1,6 @@ # Project information -site_name: 'WB-Toolbox' -site_description: 'A Simulink Toolbox for Whole Body Control' +site_name: 'WBT' +site_description: 'A Simulink Toolbox for Whole-Body Control' site_author: 'Diego Ferigo' # Repository @@ -23,7 +23,7 @@ theme: icon: dashboard # Copyright -copyright: 'Copyright © 2018 Istituto Italiano di Tecnologia - iCub Facility' +copyright: 'Copyright © 2019 Istituto Italiano di Tecnologia - iCub Facility' # Extensions markdown_extensions: @@ -55,14 +55,14 @@ markdown_extensions: nav: - Home: 'index.md' - Install: 'install.md' - - Getting started: 'getting-started.md' - Troubleshooting: 'troubleshooting.md' - Documentation [master]: https://robotology.github.io/wb-toolbox/doxygen/ - Documentation [devel]: https://robotology.github.io/wb-toolbox/devel/doxygen/ - - Developer area: - - "Tutorial: create a new library": 'create_new_library.md' - - Add a new block: 'create_new_block.md' - - Simulink Tricks: 'sim_tricks.md' + - Tutorials: + - Introduction: tutorial_introduction.md + - Basic Simulink simulation: simulink_basic.md + - Use model blocks: simulink_model_blocks.md + - Simulink and Gazebo: simulink_gazebo.md - Migration: - From WBI-Toolbox: 'Migration_from_WBI-Toolbox_1.0.md' - From WB-Toolbox 2.0: 'Migration_from_WB-Toolbox_2.0.md' diff --git a/doc/release/template.md b/doc/release/template.md index 8f2d8da9c..5e07833fe 100644 --- a/doc/release/template.md +++ b/doc/release/template.md @@ -1,4 +1,4 @@ -# WB-Toolbox (YYYY-MM-DD) Release Notes {[`#vX.Y`](https://github.com/robotology/wb-toolbox/releases/tag/vX.Y)} +# Whole-Body Toolbox (YYYY-MM-DD) Release Notes {[`#vX.Y`](https://github.com/robotology/wb-toolbox/releases/tag/vX.Y)} [Description] @@ -6,19 +6,15 @@ ## New Features -### `WBToolboxLibrary` +### `WBToolboxBase` -### `WBToolboxMex` - -### `WBToolboxCoder` +### `WBToolbox` ## Bug Fixes -### `WBToolboxLibrary` - -### `WBToolboxMex` +### `WBToolboxBase` -### `WBToolboxCoder` +### `WBToolbox` ## Contributors diff --git a/doc/release/v5.md b/doc/release/v5.md index 8f2d8da9c..0e4cf0d53 100644 --- a/doc/release/v5.md +++ b/doc/release/v5.md @@ -1,28 +1,23 @@ -# WB-Toolbox (YYYY-MM-DD) Release Notes {[`#vX.Y`](https://github.com/robotology/wb-toolbox/releases/tag/vX.Y)} +# Whole-Body Toolbox (YYYY-MM-DD) Release Notes {[`#v5`](https://github.com/robotology/wb-toolbox/releases/tag/v5)} -[Description] +The `v5` release of Whole-Body Toolbox brings a big change in the architecture: the core of the toolbox became a standalone repository hosted at [robotology/blockfactory](https://github.com/robotology/blockfactory). This repository will only provide a BlockFactory plugin. -## Important Changes - -## New Features - -### `WBToolboxLibrary` - -### `WBToolboxMex` +More in detail, starting with this release `WBT` will contain only the following components: -### `WBToolboxCoder` +- `WBToolboxBase` provides the base classes that simplify the interfacing with iDynTree and YARP libraries. +- `WBToolbox` is the real BlockFactory plugin, and it contains all the implementations of the `blockfactory::core::Block` interface. -## Bug Fixes - -### `WBToolboxLibrary` - -### `WBToolboxMex` +## Important Changes -### `WBToolboxCoder` +- The Core, Mex and Coder components have been removed +- New dependency: [robotology/blockfactory](https://github.com/robotology/blockfactory) +- If Matlab is found, the Simulink Library that wraps the block classes is installed ## Contributors -This is a list of people that contributed to this release (generated from the git history using `git shortlog -ens --no-merges vW.X..vY.Z`): +This is a list of people that contributed to this release (generated from the git history using `git shortlog -ens --no-merges v4..v5`): ``` +42 Diego Ferigo + 1 Silvio Traversaro ``` diff --git a/example/.clang-format b/example/.clang-format deleted file mode 100644 index bc30ce6ff..000000000 --- a/example/.clang-format +++ /dev/null @@ -1,108 +0,0 @@ ---- -Language: Cpp -# BasedOnStyle: WebKit -AccessModifierOffset: -4 -AlignAfterOpenBracket: Align -AlignConsecutiveAssignments: false -AlignConsecutiveDeclarations: false -AlignEscapedNewlines: Left -AlignOperands: true -AlignTrailingComments: false -AllowAllParametersOfDeclarationOnNextLine: false -AllowShortBlocksOnASingleLine: false -AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: Inline -AllowShortIfStatementsOnASingleLine: false -AllowShortLoopsOnASingleLine: false -AlwaysBreakAfterDefinitionReturnType: None -AlwaysBreakAfterReturnType: None -AlwaysBreakBeforeMultilineStrings: false -AlwaysBreakTemplateDeclarations: true -BinPackArguments: false -BinPackParameters: false -BraceWrapping: - AfterClass: true - AfterControlStatement: false - AfterEnum: true - AfterFunction: true - AfterNamespace: false - AfterObjCDeclaration: false - AfterStruct: true - AfterUnion: false - BeforeCatch: true - BeforeElse: true - IndentBraces: false - SplitEmptyFunction: false - SplitEmptyRecord: false - SplitEmptyNamespace: false -BreakBeforeBinaryOperators: NonAssignment -BreakBeforeBraces: Custom -BreakBeforeInheritanceComma: true -BreakBeforeTernaryOperators: true -BreakConstructorInitializersBeforeComma: true -BreakConstructorInitializers: BeforeComma -BreakAfterJavaFieldAnnotations: false -BreakStringLiterals: true -ColumnLimit: 100 -CommentPragmas: '^ IWYU pragma:' -CompactNamespaces: false -ConstructorInitializerAllOnOneLineOrOnePerLine: false -ConstructorInitializerIndentWidth: 4 -ContinuationIndentWidth: 4 -Cpp11BracedListStyle: true -DerivePointerAlignment: false -DisableFormat: false -ExperimentalAutoDetectBinPacking: false -FixNamespaceComments: true -ForEachMacros: - - foreach - - Q_FOREACH - - BOOST_FOREACH -IncludeBlocks: Preserve -IncludeCategories: - - Regex: '^"(llvm|llvm-c|clang|clang-c)/' - Priority: 2 - - Regex: '^(<|"(gtest|gmock|isl|json)/)' - Priority: 3 - - Regex: '.*' - Priority: 1 -IncludeIsMainRegex: '(Test)?$' -IndentCaseLabels: true -IndentWidth: 4 -IndentWrappedFunctionNames: false -JavaScriptQuotes: Leave -JavaScriptWrapImports: true -KeepEmptyLinesAtTheStartOfBlocks: true -MacroBlockBegin: '' -MacroBlockEnd: '' -MaxEmptyLinesToKeep: 1 -NamespaceIndentation: All -ObjCBlockIndentWidth: 4 -ObjCSpaceAfterProperty: true -ObjCSpaceBeforeProtocolList: true -PenaltyBreakAssignment: 2 -PenaltyBreakBeforeFirstCallParameter: 19 -PenaltyBreakComment: 300 -PenaltyBreakFirstLessLess: 120 -PenaltyBreakString: 1000 -PenaltyExcessCharacter: 1000000 -PenaltyReturnTypeOnItsOwnLine: 60 -PointerAlignment: Left -ReflowComments: true -SortIncludes: true -SortUsingDeclarations: true -SpaceAfterCStyleCast: true -SpaceAfterTemplateKeyword: true -SpaceBeforeAssignmentOperators: true -SpaceBeforeParens: ControlStatements -SpaceInEmptyParentheses: false -SpacesBeforeTrailingComments: 1 -SpacesInAngles: false -SpacesInContainerLiterals: true -SpacesInCStyleCastParentheses: false -SpacesInParentheses: false -SpacesInSquareBrackets: false -Standard: Cpp11 -TabWidth: 4 -UseTab: Never -... diff --git a/example/CMakeLists.txt b/example/CMakeLists.txt deleted file mode 100644 index 374aad6b9..000000000 --- a/example/CMakeLists.txt +++ /dev/null @@ -1,56 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(ExampleToolbox LANGUAGES CXX VERSION 0.1) - -# C++ standard -set(CMAKE_CXX_STANDARD 11) -set(CMAKE_CXX_STANDARD_REQUIRED ON) - -# Export all symbols in Windows -set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) - -# Utility for defining install folders -include(GNUInstallDirs) - -# Tweak linker flags in Linux. -# Matlab is very strict on missing symbols and by default ld do not warn if -# something is missing. -if(UNIX AND NOT APPLE) - get_filename_component(LINKER_BIN ${CMAKE_LINKER} NAME) - if(${LINKER_BIN} STREQUAL "ld") - set(CMAKE_SHARED_LINKER_FLAGS "-Wl,--unresolved-symbols=report-all") - endif() -endif() - -# =========== -# C++ LIBRARY -# =========== - -# Find the needed WBToolbox components: -# - ToolboxCore contains the core classes such as Block and Signal -# - ToolboxMex is required at runtime for loading the library from Simulink -find_package(WBToolbox 4 REQUIRED COMPONENTS ToolboxCore ToolboxMex) - -# Find the project for the multiplatform support of plugin libraries. -# It is shipped with WBToolbox and it should be already installed in your system. -find_package(shlibpp REQUIRED) - -# Create the plugin library. This must be a SHARED library. -add_library(ExampleToolbox SHARED - include/SignalMath.h - src/SignalMath.cpp - src/Factory.cpp) - -# Manually set the name of the output library. This is not required and it -# is done only for sake of clarity. -set_target_properties(ExampleToolbox PROPERTIES - OUTPUT_NAME "ExampleToolbox") - -# Link the library with the Core component containing the core classes -# and the target that provides the plugin support -target_link_libraries(ExampleToolbox - WBToolbox::Core shlibpp::shlibpp) - -# Setup the include directories -target_include_directories(ExampleToolbox PUBLIC - $ - $) diff --git a/example/README.md b/example/README.md deleted file mode 100644 index 5b5d269a0..000000000 --- a/example/README.md +++ /dev/null @@ -1,28 +0,0 @@ -# Toolbox Example - -This folder contains a simple toolbox with a test block. It can be used as starting point to create a new toolbox based on the machinery provided by Whole Body Toolbox. - -It is also the result of the tutorial [Toolbox Example](https://robotology.github.io/wb-toolbox/mkdocs/create_new_library/). - -### Instructions - -Execute the following commands to build the example: - -```bash -cd example/ -mkdir build -cd build -cmake .. -cmake --build . -``` - -On a Linux distribution, you can test the Toolbox Example library as follows: - -```bash -cd example/ -export LD_LIBRARY_PATH=$(pwd)/build:$LD_LIBRARY_PATH -export MATLABPATH=$(pwd)/matlab:$MATLABPATH -matlab -``` - -Finally, open the `Model.mdl` file and run it. \ No newline at end of file diff --git a/example/include/SignalMath.h b/example/include/SignalMath.h deleted file mode 100644 index 4a3fff687..000000000 --- a/example/include/SignalMath.h +++ /dev/null @@ -1,48 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef EXAMPLE_SIGNALMATH_H -#define EXAMPLE_SIGNALMATH_H - -#include -#include - -#include -#include - -namespace example { - class SignalMath; -} // namespace example - -class example::SignalMath : public wbt::Block -{ -private: - enum class Operation - { - ADDITION, - SUBTRACTION, - MULTIPLICATION, - }; - - Operation m_operation; - -public: - static const std::string ClassName; - - SignalMath() = default; - ~SignalMath() override = default; - - unsigned numberOfParameters() override; - bool parseParameters(wbt::BlockInformation* blockInfo) override; - bool configureSizeAndPorts(wbt::BlockInformation* blockInfo) override; - bool initialize(wbt::BlockInformation* blockInfo) override; - bool output(const wbt::BlockInformation* blockInfo) override; - bool terminate(const wbt::BlockInformation* blockInfo) override; -}; - -#endif // EXAMPLE_SIGNALMATH_H diff --git a/example/matlab/ExampleToolbox.slx b/example/matlab/ExampleToolbox.slx deleted file mode 100644 index 3ca0ffbda..000000000 Binary files a/example/matlab/ExampleToolbox.slx and /dev/null differ diff --git a/example/matlab/Model.mdl b/example/matlab/Model.mdl deleted file mode 100644 index 65cd53201..000000000 --- a/example/matlab/Model.mdl +++ /dev/null @@ -1,1174 +0,0 @@ -Model { - Name "Model" - Version 9.1 - SavedCharacterEncoding "UTF-8" - GraphicalInterface { - NumRootInports 0 - NumRootOutports 0 - ParameterArgumentNames "" - ComputedModelVersion "1.12" - NumModelReferences 0 - NumTestPointedSignals 0 - NumProvidedFunctions 0 - NumRequiredFunctions 0 - NumResetEvents 0 - HasInitializeEvent 0 - HasTerminateEvent 0 - IsExportFunctionModel 0 - NumParameterArguments 0 - NumExternalFileReferences 1 - ExternalFileReference { - Reference "ExampleToolbox/Signal Math" - Path "Model/w// mask" - SID "8" - Type "LIBRARY_BLOCK" - } - OrderedModelArguments 1 - } - DiagnosticSuppressor "on" - SuppressorTable "22 serialization::archive 11 0 6 0 0 0 1 0" - Notes "" - NotesPlugin "on" - LogicAnalyzerGraphicalSettings "" - LogicAnalyzerPlugin "on" - LogicAnalyzerSignalOrdering "" - CustomCodeFunctionData "" - SLCCPlugin "on" - ScopeRefreshTime 0.035000 - OverrideScopeRefreshTime on - DisableAllScopes off - DataTypeOverride "UseLocalSettings" - DataTypeOverrideAppliesTo "AllNumericTypes" - MinMaxOverflowLogging "UseLocalSettings" - MinMaxOverflowArchiveMode "Overwrite" - 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 [200.0, 70.0, 1575.0, 918.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 [1521.0, 688.0] - ZoomFactor [1.5] - Offset [-87.952221478008482, -101.92174592542986] - } - Object { - $PropName "DockComponentsInfo" - $ObjectID 6 - $ClassName "Simulink.DockComponentInfo" - Type "GLUE2:PropertyInspector" - ID "Property Inspector" - Visible [0] - CreateCallback "" - UserData "" - Floating [0] - DockPosition "Right" - Width [640] - Height [480] - } - WindowState "AAAA/wAAAAD9AAAAAgAAAAAAAAC9AAAB+PwCAAAAA/sAAAAWAEQAbwBjAGsAVwBpAGQAZwBlAHQAMwEAAAAxAAAB+AAAA" - "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" - "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAAAP////8AAAB4AP///wAAAAEAAAAAAAAAAPwCA" - "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAMQD///8AAAYXAAAC9gAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" - "////wEAAACG/////wAAAAAAAAAA/////wEAAADu/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAF0/////wAAAAAAAAAA/" - "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAOq/////wAAAAAAAAAA/" - "////wEAAAPf/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA" - } - } - HideAutomaticNames off - Created "Wed Aug 08 16:01:42 2018" - Creator "dferigo" - UpdateHistory "UpdateHistoryNever" - ModifiedByFormat "%" - LastModifiedBy "dferigo" - ModifiedDateFormat "%" - LastModifiedDate "Fri Aug 10 16:26:57 2018" - RTWModifiedTimeStamp 455819216 - ModelVersionFormat "1.%" - SampleTimeColors off - SampleTimeAnnotations off - LibraryLinkDisplay "all" - 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 - ShowSubsystemDomainSpec off - ExecutionContextIcon off - ShowLinearizationAnnotations on - ShowVisualizeInsertedRTB on - ShowMarkup on - BlockNameDataTip off - BlockParametersDataTip off - BlockDescriptionStringDataTip off - BlockVariantConditionDataTip off - ToolBar on - StatusBar on - BrowserShowLibraryLinks off - FunctionConnectors off - BrowserLookUnderMasks off - MultiThreadCoSim on - SimulationMode "normal" - VisualizeLoggedSignalsWhenLoggingToFile off - PauseTimes "5" - NumberOfSteps 1 - SnapshotBufferSize 10 - SnapshotInterval 10 - NumberOfLastSnapshots 0 - EnablePacing off - PacingRate 1 - 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_ "Model" - overrideMode_ [0.0] - Array { - Type "Cell" - Dimension 1 - Cell "Model" - PropName "logAsSpecifiedByModels_" - } - Array { - Type "Cell" - Dimension 1 - Cell [] - PropName "logAsSpecifiedByModelsSSIDs_" - } - } - 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.18.0" - DisabledProps [] - Description "" - Array { - Type "Handle" - Dimension 9 - Simulink.SolverCC { - $ObjectID 9 - Version "1.18.0" - DisabledProps [] - Description "" - StartTime "0.0" - StopTime "10.0" - AbsTol "auto" - AutoScaleAbsTol "on" - FixedStep "auto" - InitialStep "auto" - MaxOrder 5 - ZcThreshold "auto" - ConsecutiveZCsStepRelTol "10*128*eps" - MaxConsecutiveZCs "1000" - ExtrapolationOrder 4 - NumberNewtonIterations 1 - MaxStep "auto" - MinStep "auto" - MaxConsecutiveMinStep "1" - RelTol "1e-3" - EnableMultiTasking off - ConcurrentTasks off - Solver "VariableStepAuto" - SolverName "VariableStepAuto" - 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 [] - DecoupledContinuousIntegration off - MinimalZcImpactIntegration off - } - Simulink.DataIOCC { - $ObjectID 10 - Version "1.18.0" - DisabledProps [] - Description "" - Decimation "1" - ExternalInput "[t, u]" - FinalStateName "xFinal" - InitialState "xInitial" - LimitDataPoints off - MaxDataPoints "1000" - LoadExternalInput off - LoadInitialState off - SaveFinalState off - SaveCompleteFinalSimState off - SaveFormat "Dataset" - SignalLoggingSaveFormat "Dataset" - 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.18.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 128 - LocalBlockOutputs on - RollThreshold 5 - StateBitsets off - DataBitsets off - ActiveStateOutputEnumStorageType "Native Integer" - ZeroExternalMemoryAtStartup on - ZeroInternalMemoryAtStartup on - InitFltsAndDblsToZero off - NoFixptDivByZeroProtection off - EfficientFloat2IntCast off - EfficientMapNaN2IntZero on - LifeSpan "auto" - MaxStackSize "Inherit from target" - BufferReusableBoundary on - SimCompilerOptimization "off" - AccelVerboseBuild off - OptimizeBlockOrder "off" - OptimizeDataStoreBuffers on - BusAssignmentInplaceUpdate on - DifferentSizesBufferReuse off - OptimizationLevel "level2" - OptimizationPriority "Balanced" - OptimizationCustomize on - UseRowMajorAlgorithm off - LabelGuidedReuse off - } - Simulink.DebuggingCC { - $ObjectID 12 - Version "1.18.0" - Array { - Type "Cell" - Dimension 1 - Cell "UseOnlyExistingSharedCode" - PropName "DisabledProps" - } - Description "" - RTPrefix "error" - ConsistencyChecking "none" - ArrayBoundsChecking "none" - SignalInfNanChecking "none" - StringTruncationChecking "error" - 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" - ExportedTasksRateTransMsg "none" - 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 "error" - 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 "none" - SimStateInterfaceChecksumMismatchMsg "warning" - SimStateOlderReleaseMsg "error" - ChecksumConsistencyForSSReuse "none" - LibraryContextMissingDiagnostic "none" - InitInArrayFormatMsg "warning" - StrictBusMsg "ErrorLevel1" - BusNameAdapt "WarnAndRepair" - NonBusSignalsTreatedAsBus "none" - SymbolicDimMinMaxWarning "warning" - LossOfSymbolicDimsSimulationWarning "warning" - LossOfSymbolicDimsCodeGenerationWarning "error" - SymbolicDimsDataTypeCodeGenerationDiagnostic "error" - BlockIODiagnostic "none" - SFUnusedDataAndEventsDiag "warning" - SFUnexpectedBacktrackingDiag "error" - SFInvalidInputDataAccessInChartInitDiag "warning" - SFNoUnconditionalDefaultTransitionDiag "error" - SFTransitionOutsideNaturalParentDiag "warning" - SFUnreachableExecutionPathDiag "warning" - SFUndirectedBroadcastEventsDiag "warning" - SFTransitionActionBeforeConditionDiag "warning" - SFOutputUsedAsStateInMooreChartDiag "error" - SFTemporalDelaySmallerThanSampleTimeDiag "warning" - SFSelfTransitionDiag "warning" - SFExecutionAtInitializationDiag "warning" - SFMachineParentedDataDiag "warning" - IntegerSaturationMsg "warning" - AllowedUnitSystems "all" - UnitsInconsistencyMsg "warning" - AllowAutomaticUnitConversions on - RCSCRenamedMsg "warning" - RCSCObservableMsg "warning" - ForceCombineOutputUpdateInSim off - UnitDatabase "" - } - Simulink.HardwareCC { - $ObjectID 13 - Version "1.18.0" - DisabledProps [] - Description "" - ProdBitPerChar 8 - ProdBitPerShort 16 - ProdBitPerInt 32 - ProdBitPerLong 32 - ProdBitPerLongLong 64 - ProdBitPerFloat 32 - ProdBitPerDouble 64 - ProdBitPerPointer 64 - ProdBitPerSizeT 64 - ProdBitPerPtrDiffT 64 - ProdLargestAtomicInteger "Char" - ProdLargestAtomicFloat "Float" - ProdIntDivRoundTo "Zero" - ProdEndianess "LittleEndian" - ProdWordSize 64 - ProdShiftRightIntArith on - ProdLongLongMode off - ProdHWDeviceType "Intel->x86-64 (Windows64)" - 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.18.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 on - SupportModelReferenceSimTargetCustomCode off - } - Simulink.SFSimCC { - $ObjectID 15 - Version "1.18.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.18.0" - Array { - Type "Cell" - Dimension 16 - 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" - Cell "CodeExecutionProfiling" - Cell "CodeProfilingSaveOptions" - Cell "CodeProfilingInstrumentation" - 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 "" - CustomFFTCallback "" - 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.18.0" - Array { - Type "Cell" - Dimension 28 - Cell "IgnoreCustomStorageClasses" - Cell "IgnoreTestpoints" - Cell "BlockCommentType" - 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 "CustomSymbolStrEmxType" - Cell "CustomSymbolStrEmxFcn" - Cell "CustomUserTokenString" - Cell "ReqsInCode" - PropName "DisabledProps" - } - Description "" - Comment "" - ForceParamTrailComments off - GenerateComments on - CommentStyle "Auto" - IgnoreCustomStorageClasses on - IgnoreTestpoints off - MaxIdLength 31 - PreserveName off - PreserveNameWithParent off - ShowEliminatedStatement off - OperatorAnnotations off - SimulinkDataObjDesc off - SFDataObjDesc off - MATLABFcnDesc off - MangleLength 1 - SharedChecksumLength 8 - 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 - BlockCommentType "BlockPathComment" - StateflowObjectComments off - MATLABSourceComments off - EnableCustomComments off - InternalIdentifierFile "" - InternalIdentifier "Shortened" - InlinedPrmAccess "Literals" - ReqsInCode off - UseSimReservedNames off - ReservedNameArray [] - } - Simulink.GRTTargetCC { - $BackupClass "Simulink.TargetCC" - $ObjectID 18 - Version "1.18.0" - 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" - } - Description "" - TargetFcnLib "ansi_tfl_table_tmw.mat" - TargetLibSuffix "" - TargetPreCompLibLocation "" - GenFloatMathFcnCalls "NOT IN USE" - TargetLangStandard "C99 (ISO)" - CodeReplacementLibrary "None" - UtilityFuncGeneration "Auto" - MultiwordTypeDef "System defined" - MultiwordLength 2048 - DynamicStringBufferSize 256 - GenerateFullHeader on - InferredTypesCompatibility off - ExistingSharedCode "" - GenerateSampleERTMain off - GenerateTestInterfaces off - ModelReferenceCompliant on - ParMdlRefBuildCompliant on - CompOptLevelCompliant on - ConcurrentExecutionCompliant on - IncludeMdlTerminateFcn on - GeneratePreprocessorConditionals "Use local settings" - CombineOutputUpdateFcns on - CombineSignalStateStructs off - SuppressErrorStatus 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 off - GenerateAllocFcn off - UseToolchainInfoCompliant on - GenerateSharedConstants on - CoderGroups [] - AccessMethods [] - LookupTableObjectStructAxisOrder "1,2,3,4,..." - LUTObjectStructOrderExplicitValues "Size,Breakpoints,Table" - LUTObjectStructOrderEvenSpacing "Size,Breakpoints,Table" - ArrayLayout "Column-major" - ERTHeaderFileRootName "$R$E" - ERTSourceFileRootName "$R$E" - ERTDataFileRootName "$R_data" - 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.18.0" - DisabledProps [] - Description "Simulink Coverage Configuration Component" - Name "Simulink Coverage" - CovEnable off - CovScope "EntireSystem" - CovIncludeTopModel on - RecordCoverage off - CovPath "/" - CovSaveName "covdata" - CovCompData "" - CovMetricSettings "dwe" - CovFilter "" - CovHTMLOptions "" - CovNameIncrementing off - CovHtmlReporting off - CovForceBlockReductionOff on - CovEnableCumulative on - CovSaveCumulativeToWorkspaceVar off - CovSaveSingleToWorkspaceVar off - CovCumulativeVarName "covCumulativeData" - CovCumulativeReport off - CovSaveOutputData on - CovOutputDir "slcov_output/$ModelName$" - CovDataFileName "$ModelName$_cvdata" - CovShowResultsExplorer on - CovReportOnPause on - CovModelRefEnable "off" - CovModelRefExcluded "" - CovExternalEMLEnable on - CovSFcnEnable on - CovBoundaryAbsTol 1e-05 - CovBoundaryRelTol 0.01 - CovUseTimeInterval off - CovStartTime 0 - CovStopTime 0 - CovMcdcMode "Masking" - } - PropName "Components" - } - Name "Configuration" - ExtraOptions "" - CurrentDlgPage "Solver" - ConfigPrmDlgPosition [ 145, 177, 1135, 847 ] - } - 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 - HideAutomaticName on - BlockRotation 0 - BlockMirror off - } - AnnotationDefaults { - HorizontalAlignment "left" - VerticalAlignment "top" - ForegroundColor "black" - BackgroundColor "white" - DropShadow off - FontName "Helvetica" - FontSize 10 - FontWeight "normal" - FontAngle "normal" - MarkupType "model" - UseDisplayTextAsClickCallback off - AnnotationType "note_annotation" - FixedHeight off - FixedWidth off - Interpreter "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 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 Display - Format "short" - Decimation "10" - Floating off - Lockdown off - SampleTime "-1" - } - Block { - BlockType S-Function - FunctionName "system" - SFunctionModules "''" - PortCounts "[]" - MultiThreadCoSim "auto" - } - } - System { - Name "Model" - Location [200, 70, 1775, 988] - Open on - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "150" - ReportName "simulink-default.rpt" - SIDHighWatermark "11" - Block { - BlockType Constant - Name "Constant" - SID "2" - Position [175, 185, 260, 215] - ZOrder 3 - NamePlacement "alternate" - Value "[1, 2, 3, 4, 5]" - Port { - PortNumber 1 - Name "input1" - } - } - Block { - BlockType Constant - Name "Constant1" - SID "3" - Position [175, 220, 260, 250] - ZOrder 4 - Value "[2, 2, 2, 2, 2]" - Port { - PortNumber 1 - Name "input2" - } - } - Block { - BlockType Constant - Name "Constant2" - SID "9" - Position [175, 0, 260, 30] - ZOrder 10 - NamePlacement "alternate" - Value "[1, 2, 3, 4, 5]" - } - Block { - BlockType Constant - Name "Constant3" - SID "10" - Position [175, 35, 260, 65] - ZOrder 11 - Value "[2, 2, 2, 2, 2]" - } - Block { - BlockType Display - Name "Display" - SID "4" - Ports [1] - Position [500, 167, 580, 273] - ZOrder 5 - Decimation "1" - } - Block { - BlockType Display - Name "Display1" - SID "11" - Ports [1] - Position [500, -18, 580, 88] - ZOrder 12 - Decimation "1" - } - Block { - BlockType Reference - Name "w/ mask" - SID "8" - Ports [2, 1] - Position [335, -1, 470, 66] - ZOrder 9 - LibraryVersion "1.8" - SourceBlock "ExampleToolbox/Signal Math" - SourceType "" - SourceProductName "Example Toolbox" - MultiThreadCoSim "auto" - operation "Multiplication" - } - Block { - BlockType S-Function - Name "w/o mask" - SID "7" - Ports [2, 1] - Position [370, 184, 430, 251] - ZOrder 8 - FunctionName "WBToolbox" - Parameters "'SignalMath','ExampleToolbox','Addition'" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Port { - PortNumber 1 - Name "output" - } - } - Line { - Name "input1" - ZOrder 11 - Labels [1, 0] - SrcBlock "Constant" - SrcPort 1 - DstBlock "w/o mask" - DstPort 1 - } - Line { - Name "input2" - ZOrder 12 - Labels [1, 1] - SrcBlock "Constant1" - SrcPort 1 - DstBlock "w/o mask" - DstPort 2 - } - Line { - Name "output" - ZOrder 13 - Labels [1, 1] - SrcBlock "w/o mask" - SrcPort 1 - DstBlock "Display" - DstPort 1 - } - Line { - ZOrder 14 - SrcBlock "Constant2" - SrcPort 1 - DstBlock "w/ mask" - DstPort 1 - } - Line { - ZOrder 15 - SrcBlock "Constant3" - SrcPort 1 - DstBlock "w/ mask" - DstPort 2 - } - Line { - ZOrder 16 - SrcBlock "w/ mask" - SrcPort 1 - DstBlock "Display1" - DstPort 1 - } - } -} diff --git a/example/matlab/slblocks.m b/example/matlab/slblocks.m deleted file mode 100644 index 1fc5e2d8c..000000000 --- a/example/matlab/slblocks.m +++ /dev/null @@ -1,11 +0,0 @@ -% Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT). All rights reserved. -% This software may be modified and distributed under the terms of the -% GNU Lesser General Public License v2.1 or any later version. - -function blkStruct = slblocks - -Browser.Library = 'ExampleToolbox'; -Browser.Name = 'Example Toolbox'; -Browser.IsFlat = 0; - -blkStruct.Browser = Browser; \ No newline at end of file diff --git a/example/src/Factory.cpp b/example/src/Factory.cpp deleted file mode 100644 index cd3db4abe..000000000 --- a/example/src/Factory.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "SignalMath.h" - -// Class factory API -#include - -// Add the example::SignalMath class to the plugin factory -SHLIBPP_DEFINE_SHARED_SUBCLASS(SignalMath, example::SignalMath, wbt::Block); diff --git a/example/src/SignalMath.cpp b/example/src/SignalMath.cpp deleted file mode 100644 index d49635d8c..000000000 --- a/example/src/SignalMath.cpp +++ /dev/null @@ -1,166 +0,0 @@ -#include "SignalMath.h" - -#include -#include -#include - -using namespace example; - -unsigned SignalMath::numberOfParameters() -{ - // The base wbt::Block class needs parameters (e.g. the ClassName). - // You must specify here how many more parameters this class needs. - // Our example needs just one more: the operation to perform. - return Block::numberOfParameters() + 1; -} - -// This method should let BlockInformation know the parameters metadata -bool SignalMath::parseParameters(wbt::BlockInformation* blockInfo) -{ - // Initialize information for our only parameter - int rows = 1; - int cols = 1; - std::string name = "Operation"; // This label is used to access the param later - unsigned index = Block::numberOfParameters(); // Indices start from 0 - wbt::ParameterType type = wbt::ParameterType::STRING; - - // Create the parameter - wbt::ParameterMetadata parameterMetadata(type, index, rows, cols, name); - - // Add the parameter metadata into the BlockInformation - if (!blockInfo->addParameterMetadata(parameterMetadata)) { - wbtError << "Failed to store parameter metadata"; - return false; - } - - // Ask to the BlockInformation interface to parse the parameters and store them into - // the m_parameters variable. This variable is contained in the wbt::Block class. - bool paramParsedOk = blockInfo->parseParameters(m_parameters); - - // Return the outcome of the parameter parsing. - // If the parsing fails, the execution stops. - return paramParsedOk; -} - -// Keep in mind that after this step, all the allocated memory will be deleted. -// Memory persistency is guaranteed starting from the initialize method. -bool SignalMath::configureSizeAndPorts(wbt::BlockInformation* blockInfo) -{ - // The base wbt::Block class need to be configured - if (!wbt::Block::configureSizeAndPorts(blockInfo)) { - return false; - } - - // Create data about input and output ports. - wbt::BlockInformation::PortData input1; - wbt::BlockInformation::PortData input2; - wbt::BlockInformation::PortData output; - input1 = {/*portIndex=*/0, std::vector{wbt::Signal::DynamicSize}, wbt::DataType::DOUBLE}; - input2 = {/*portIndex=*/1, std::vector{wbt::Signal::DynamicSize}, wbt::DataType::DOUBLE}; - output = {/*portIndex=*/0, std::vector{wbt::Signal::DynamicSize}, wbt::DataType::DOUBLE}; - - // Populate a structure with the overall input / output data - wbt::BlockInformation::IOData ioData; - ioData.input.push_back(input1); - ioData.input.push_back(input2); - ioData.output.push_back(output); - - // Store this data into the BlockInformation - if (!blockInfo->setIOPortsData(ioData)) { - wbtError << "Failed to configure input / output ports"; - return false; - } - - return true; -} - -bool SignalMath::initialize(wbt::BlockInformation* blockInfo) -{ - // The base wbt::Block class need to be initialized - if (!Block::initialize(blockInfo)) { - return false; - } - - // Parse the parameters - if (!SignalMath::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; - return false; - } - - // Read the Operation parameter and store it as a private member - std::string operation; - if (!m_parameters.getParameter("Operation", operation)) { - wbtError << "Failed to parse Operation parameter"; - return false; - } - - // Check the content of the parameter - if (operation == "Addition") { - m_operation = Operation::ADDITION; - } - else if (operation == "Subtraction") { - m_operation = Operation::SUBTRACTION; - } - else if (operation == "Multiplication") { - m_operation = Operation::MULTIPLICATION; - ; - } - else { - wbtError << "Operation " << operation << " not supported"; - return false; - } - - // Check that the size of the input signals match - if (blockInfo->getInputPortWidth(/*index=*/0) != blockInfo->getInputPortWidth(/*index=*/1)) { - wbtError << "Input signals widths do not match"; - return false; - } - - return true; -} - -bool SignalMath::output(const wbt::BlockInformation* blockInfo) -{ - // Get the input signals - wbt::InputSignalPtr input1 = blockInfo->getInputPortSignal(/*index=*/0); - wbt::InputSignalPtr input2 = blockInfo->getInputPortSignal(/*index=*/1); - - // Get the output signal - wbt::OutputSignalPtr output = blockInfo->getOutputPortSignal(/*index=*/0); - - // Check the signal validity - if (!input1 || !input2 || !output) { - wbtError << "Signals not valid"; - return false; - } - - // Check the width of the output signal. - // This is recommended for dynamically sized signals. - if (output->getWidth() != input1->getWidth()) { - wbtError << "Output signal has a width of " << output->getWidth() - << " while input signals have a width of " << input1->getWidth(); - return false; - } - - // Perform the given operation - for (unsigned i = 0; i < output->getWidth(); ++i) { - switch (m_operation) { - case Operation::ADDITION: - output->set(i, input1->get(i) + input2->get(i)); - break; - case Operation::SUBTRACTION: - output->set(i, input1->get(i) - input2->get(i)); - break; - case Operation::MULTIPLICATION: - output->set(i, input1->get(i) * input2->get(i)); - break; - } - } - - return true; -} - -bool SignalMath::terminate(const wbt::BlockInformation* /*blockInfo*/) -{ - return true; -} diff --git a/matlab/CMakeLists.txt b/matlab/CMakeLists.txt index fd4402397..d9ea3dcd6 100644 --- a/matlab/CMakeLists.txt +++ b/matlab/CMakeLists.txt @@ -54,15 +54,7 @@ configure_file( ${CMAKE_BINARY_DIR}/toolbox/setupForMatlabDebug.m @ONLY) -#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 -# 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 the Simulink Library install( FILES ${CMAKE_CURRENT_SOURCE_DIR}/library/exported/WBToolboxLibrary.slx DESTINATION ${WBTOOLBOX_SHARE_DIR}) diff --git a/matlab/export_library.m b/matlab/export_library.m index a06fd2059..2e36e2185 100644 --- a/matlab/export_library.m +++ b/matlab/export_library.m @@ -24,14 +24,11 @@ set_param(libraryName, 'Lock', 'on'); end - % Export the library + % Export the library. It must be in slx otherwise it will not show up in + % the Simulink Library browser. fprintf('\nExporting for 2014b\n'); save_system(libraryName, 'WBToolboxLibrary', 'ExportToVersion', 'R2014B_SLX'); 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); diff --git a/matlab/library/WBToolboxLibrary_repository.mdl b/matlab/library/WBToolboxLibrary_repository.mdl index 45de2976b..920dd57f0 100644 --- a/matlab/library/WBToolboxLibrary_repository.mdl +++ b/matlab/library/WBToolboxLibrary_repository.mdl @@ -27,7 +27,7 @@ Library { $ObjectID 2 $ClassName "Simulink.WindowInfo" IsActive [1] - Location [832.0, 225.0, 2868.0, 1689.0] + Location [0.0, 27.0, 1920.0, 1053.0] Object { $PropName "ModelBrowserInfo" $ObjectID 3 @@ -50,10 +50,10 @@ Library { $ClassName "Simulink.EditorInfo" IsActive [1] ViewObjType "SimulinkSubsys" - LoadSaveID "224" - Extents [2830.0, 1534.0] - ZoomFactor [10.0] - Offset [557.365625, 208.45] + LoadSaveID "192" + Extents [1882.0, 849.0] + ZoomFactor [2.1893333333333334] + Offset [-79.803440925700272, -12.394640682095002] } Object { $PropName "DockComponentsInfo" @@ -73,7 +73,7 @@ Library { "AAAAAAA+wAAABYARABvAGMAawBXAGkAZABnAGUAdAA0AAAAAAD/////AAAAAAAAAAD7AAAAUgBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0Ac" "ABvAG4AZQBuAHQALwBHAEwAVQBFADIAIAB0AHIAZQBlACAAYwBvAG0AcABvAG4AZQBuAHQAAAAANQAAAooAAAB4AP///wAAAAEAAAAAAAAAAPwCA" "AAAAfsAAABUAEcATABVAEUAMgA6AFAAcgBvAHAAZQByAHQAeQBJAG4AcwBwAGUAYwB0AG8AcgAvAFAAcgBvAHAAZQByAHQAeQAgAEkAbgBzAHAAZ" - "QBjAHQAbwByAAAAAAD/////AAAAMQD///8AAAs0AAAGRAAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" + "QBjAHQAbwByAAAAAAD/////AAAAMQD///8AAAeAAAADlwAAAAEAAAACAAAAAQAAAAL8AAAAAQAAAAIAAAAP/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" "////wEAAACG/////wAAAAAAAAAA/////wEAAADu/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/" "////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wAAAAAA/////wAAAAAAAAAA/////wEAAAFE/////wAAAAAAAAAA/" @@ -87,9 +87,9 @@ Library { ModifiedByFormat "%" LastModifiedBy "dferigo" ModifiedDateFormat "%" - LastModifiedDate "Tue Aug 21 13:59:32 2018" - RTWModifiedTimeStamp 456760771 - ModelVersionFormat "1.%" + LastModifiedDate "Wed Dec 05 14:25:23 2018" + RTWModifiedTimeStamp 465920717 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "all" @@ -1105,7 +1105,7 @@ Library { } System { Name "WBToolboxLibrary_repository" - Location [832, 225, 3700, 1914] + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -1120,16 +1120,16 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "626" + ZoomFactor "416" ReportName "simulink-default.rpt" SIDHighWatermark "1847" Block { BlockType SubSystem - Name "Utilities" - SID "192" + Name "Actuators" + SID "224" Ports [] - Position [364, 17, 462, 114] - ZOrder -1 + Position [250, 16, 348, 113] + ZOrder -17 ForegroundColor "white" DropShadow on RequestExecContextInheritance off @@ -1137,11 +1137,11 @@ Library { $PropName "MaskObject" $ObjectID 20 $ClassName "Simulink.Mask" - Display "image(imread('utilities.png'))" + Display "image(imread('wholeBodyActuators.png'),'center')" } System { - Name "Utilities" - Location [832, 225, 3700, 1914] + Name "Actuators" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -1156,25 +1156,39 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "381" + ZoomFactor "562" Block { - BlockType SubSystem - Name "Configuration" - SID "1774" - Ports [] - Position [295, 310, 370, 340] - ZOrder 95 - InitFcn "source = get_param(gcb,'ConfigSource');\n\nif strcmp(source, 'Workspace')\n object = get_param(gcb,'" - "ConfigObject');\n WBToolbox.ConfigurationToMask(gcb, object);\nend\n\nWBToolbox.MaskToConfiguration(gcb);\n\ncle" - "ar object source" - RequestExecContextInheritance off + BlockType S-Function + Name "SetMotorParameters" + SID "1847" + Ports [1, 1] + Position [645, 290, 755, 340] + ZOrder 106 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + FunctionName "BlockFactory" + Parameters "'SetMotorParameters','WBToolbox',WBTConfigParameters,configBlockAbsName,readP,readI,readD,pid" + "Type,setKTau,kTau,setBemf,bemf" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" $ObjectID 21 $ClassName "Simulink.Mask" - SelfModifiable "on" - Display "disp('Config')" - RunInitForIconRedraw "off" + Type "SetMotorParameters" + Description "This block allows to set motor PIDs and constants such as the motor constant and the back EMF.\n" + "\nThe motor constant and the back EMF are set in the block's mask, and their values are applied during the first " + "simulation loop. The PIDs values are instead set from the input ports.\n\nAll these vectors should have a size eq" + "ual to the number of controlled joints specified in the referred Configuration block.\n\nAt the end of the simula" + "tion, the default values are restored." + 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% The booleans of the checkboxes always create problems\nreadP = uint8(readP);" + "\nreadI = uint8(readI);\nreadD = uint8(readD);" + Display "fprintf('Motor Parameters\\n(%s)',pidType);\n\nnumPorts = 0;\n\nif readP\n numPorts = numPorts +" + " 1;\n port_label('input',numPorts,'P')\nend\n\nif readI\n numPorts = numPorts + 1;\n port_label('input'," + "numPorts,'I')\nend\n\nif readD\n numPorts = numPorts + 1;\n port_label('input',numPorts,'D')\nend\n" + RunInitForIconRedraw "on" Array { Type "Simulink.MaskParameter" Dimension 8 @@ -1184,80 +1198,75 @@ Library { Array { Type "Cell" Dimension 2 - Cell "Workspace" - Cell "Mask" + Cell "Position" + Cell "Torque" 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.ConfigurationToMask(gcb,curr" - "entConfigObject);\n clear currentConfigObject;\nelseif strcmp(currentConfigSource,'Mask')\n % Switch the visib" - "ility 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.Pa" - "rameters(6).Enabled = 'on';\n h.Parameters(7).Enabled = 'on';\n h.Parameters(8).Enabled = 'on';\nend\n\nclear " - "h currentConfigSource" + 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" } Object { $ObjectID 23 - 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.ConfigurationToMask(gcb,currentConfigObject);\n\nclear currentConfigObject;" + Type "checkbox" + Name "readP" + Prompt "Enable P gains input signal" + Value "on" } Object { $ObjectID 24 - Type "edit" - Name "RobotName" - Prompt "Robot Name" - Value "'icubSim'" + Type "checkbox" + Name "readI" + Prompt "Enable I gains input signal" + Value "off" } Object { $ObjectID 25 - Type "edit" - Name "UrdfFile" - Prompt "Urdf File" - Value "'model.urdf'" + Type "checkbox" + Name "readD" + Prompt "Enable D gains input signal" + Value "off" } Object { $ObjectID 26 - Type "edit" - Name "ControlledJoints" - Prompt "Controlled Joints" - Value "{'l_elbow','l_shoulder_pitch','torso_roll'}" + Type "checkbox" + Name "setKTau" + Prompt "Set motor constant KTau" + Value "off" + Callback "h = Simulink.Mask.get(gcb);\nsetKTauValue = get_param(gcb,'setKTau');\n\nif ~strcmp(setKTauValue,'on')\n " + " h.Parameters(6).Enabled = 'off';\nelse\n h.Parameters(6).Enabled = 'on';\nend\n\nclear h setKTauValue" } Object { $ObjectID 27 Type "edit" - Name "ControlBoardsNames" - Prompt "Control Boards Names" - Value "{'left_arm','right_arm','torso'}" + Name "kTau" + Prompt "kTau" + Value "[0]" + Enabled "off" } Object { $ObjectID 28 - Type "edit" - Name "LocalName" - Prompt "Local Name" - Value "'WBT'" + Type "checkbox" + Name "setBemf" + Prompt "Set motor back EMF constant" + Value "off" + Callback "h = Simulink.Mask.get(gcb);\nsetBemfValue = get_param(gcb,'setBemf');\n\nif ~strcmp(setBemfValue,'on')\n " + " h.Parameters(8).Enabled = 'off';\nelse\n h.Parameters(8).Enabled = 'on';\nend\n\nclear h setBemfValue" } Object { $ObjectID 29 Type "edit" - Name "GravityVector" - Prompt "Gravity Vector" - Value "[0,0,-9.81]" + Name "bemf" + Prompt "bemf" + Value "[0]" + Enabled "off" } PropName "Parameters" } @@ -1279,153 +1288,190 @@ Library { Object { $ObjectID 32 Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 33 - $ClassName "Simulink.dialog.TabContainer" - Array { - Type "Simulink.dialog.Tab" - Dimension 2 - Object { - $ObjectID 34 - Prompt "From" + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 33 + Prompt "Gains" Array { Type "Simulink.dialog.parameter.Control" - Dimension 2 + Dimension 4 Object { - $ObjectID 35 + $ObjectID 34 $ClassName "Simulink.dialog.parameter.Popup" - Name "ConfigSource" + Name "pidType" } Object { - $ObjectID 36 - $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "ConfigObject" - } - PropName "DialogControls" + $ObjectID 35 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "readP" } - Name "TabFrom" + Object { + $ObjectID 36 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "readI" } Object { $ObjectID 37 - Prompt "Data" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 6 - Object { - $ObjectID 38 - PromptLocation "left" - Name "RobotName" + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "readD" + } + PropName "DialogControls" } + Name "Container3" + } + Object { + $ObjectID 38 + Prompt "Motor Torque Parameters" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 4 Object { $ObjectID 39 - PromptLocation "left" - Name "UrdfFile" + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "setKTau" } Object { $ObjectID 40 + $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" - Name "ControlledJoints" + Name "kTau" } Object { $ObjectID 41 - PromptLocation "left" - Name "ControlBoardsNames" + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "setBemf" } Object { $ObjectID 42 + $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" - Name "LocalName" - } - Object { - $ObjectID 43 - PromptLocation "left" - Name "GravityVector" + Name "bemf" } PropName "DialogControls" } - Name "TabData" + Name "Container4" + } + Object { + $ObjectID 43 + Object { + $PropName "DialogControls" + $ObjectID 44 + $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 "Control2" } - PropName "DialogControls" + Name "ToggleButtonContainer" } - Name "TabContainer" + PropName "DialogControls" } Name "ParameterGroupVar" } PropName "DialogControls" } } - System { - Name "Configuration" - Location [550, 86, 1677, 725] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 [495, 198, 565, 242] - ZOrder -1 - BackgroundColor "[0.848000, 0.128048, 0.320035]" + Name "SetMotorReferences" + SID "1791" + Ports [1] + Position [710, 210, 785, 260] + ZOrder 44 + BackgroundColor "[0.960000, 0.770000, 0.460000]" RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 44 + $ObjectID 45 $ClassName "Simulink.Mask" + Type "SetMotorReferences" + Description "This block sets motor references. The type of control mode is specified as a parameter in the b" + "lock.\n\nAssuming N as the number of motors (usually equal to internal DoFs):\n\nInput:\n - References: Vector o" + "f size N, representing the references to be sent to the robot controlled motors.\n\nParameters:\n - Control Mode" + ": The control mode. Choose one of the supplied mode.\n\nUnit of measurement:\n - PWM [-100, 100]% Duty cycle\n " + " - Current [A]\n - Back EMF\n - Torque Constant" + Initialization "refSpeed = 0;\n\n% WBBlock Initialization\n% ======================\n\ntry\n [configBlockA" + "bsName, WBTConfig] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimul" + "inkParameters;\n clear WBTConfig;\ncatch\nend" + Display "disp(get_param(gcb,'controlType'))\n%port_label('input',1,'References')" RunInitForIconRedraw "off" Object { $PropName "Parameters" - $ObjectID 45 + $ObjectID 46 $ClassName "Simulink.MaskParameter" - Type "edit" - Name "tol" - Prompt "Tolerance" - Value "1e-4" + Type "popup" + Array { + Type "Cell" + Dimension 4 + Cell "PWM" + Cell "Current" + Cell "Back EMF" + Cell "Torque Constant" + PropName "TypeOptions" + } + Name "controlType" + Prompt "Control Mode" + Value "Current" + Evaluate "off" + Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(" + "gcb,'controlType');\n\nswitch maskStr\n case 'PWM'\n set_param(gcb,'BackgroundColor', '[1, 1, 1, 1.0]" + "');\n case 'Current'\n set_param(gcb,'BackgroundColor', '[0.96, 0.77, 0.46, 1.0]');\n case 'Back E" + "MF'\n set_param(gcb,'BackgroundColor', '[0.8, 0.4, 1.0, 1.0]');\n case 'Torque Constant'\n set" + "_param(gcb,'BackgroundColor', '[0.8, 1.0, 0.4, 1.0]');\n otherwise\n error('Control Type not recogniz" + "ed.')\nend\n\nclear maskStr" + } + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 47 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 48 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 49 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 50 + $ClassName "Simulink.dialog.parameter.Popup" + Name "controlType" + } + Name "ParameterGroupVar" + } + Object { + $ObjectID 51 + Object { + $PropName "DialogControls" + $ObjectID 52 + $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 "DampPinv" - Location [0, 29, 1280, 744] + Name "SetMotorReferences" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -1440,316 +1486,119 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" + ZoomFactor "1000" Block { BlockType Inport - Name "mat" - SID "105" - Position [50, 53, 80, 67] - ZOrder -1 + Name "References" + SID "1794" + Position [55, 48, 85, 62] + ZOrder 24 IconDisplay "Port number" } Block { - BlockType Inport - Name "sigma" - SID "106" - Position [50, 93, 80, 107] - ZOrder -2 - Port "2" - IconDisplay "Port number" + BlockType S-Function + Name "S-Function" + SID "1795" + Ports [1] + Position [125, 39, 185, 71] + ZOrder 19 + FunctionName "BlockFactory" + Parameters "'SetReferences','WBToolbox',WBTConfigParameters,configBlockAbsName,controlType,refSpeed" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off } - 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" - System { - Name "Damped Pseudo Inverse" - Location [12, 45, 1279, 3773] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "1628" - 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::1627" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 107 - Outputs "1" - } - Block { - BlockType S-Function - Name " SFunction " - SID "107::1626" - Tag "Stateflow S-Function WBToolboxLibrary_repository 6" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 106 - FunctionName "sf_sfun" - PortCounts "[2 2]" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "DPinv" - } + Line { + ZOrder 1 + SrcBlock "References" + SrcPort 1 + DstBlock "S-Function" + DstPort 1 + } + } + } + Block { + BlockType SubSystem + Name "SetReferences" + SID "1767" + Ports [1] + Position [605, 210, 680, 260] + ZOrder 43 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 53 + $ClassName "Simulink.Mask" + Type "SetReferences" + 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 internal degrees of freedom configured in the asso" + "ciated Config block:\n\nInput:\n - References: Vector of size DoFs, representing the references to be sent to th" + "e robot controlled joints.\n - Reference Speed (Position): Set the reference speed in rad/s used by the trajecto" + "ry generator of the IPositionControl interface\n - Reference Acceleration (Velocity): Set the reference speed in" + " rad/s/s used by the trajectory generator of the IVelocityControl interface\n\nParameters:\n - Control Mode: The" + " control mode. Choose one of the supplied mode.\n\nUnit of measurement:\n- Position [rad]\n- Velocity [rad/s]\n- " + "Acceleration [rad/s/s]\n- Torque [Nm]" + Initialization "if (strcmp(controlType,'Position'))\n refTrajectory = refSpeed;\nelseif (strcmp(controlTyp" + "e,'Velocity'))\n refTrajectory = refAcceleration;\nelse\n refTrajectory = 0;\nend\n\n% WBBlock Initializati" + "on\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 3 + Object { + $ObjectID 54 + Type "popup" + Array { + Type "Cell" + Dimension 4 + Cell "Position" + Cell "Position Direct" + Cell "Velocity" + Cell "Torque" + PropName "TypeOptions" } - Block { - BlockType Terminator - Name " Terminator " - SID "107::1628" - Position [460, 241, 480, 259] - ZOrder 108 - } - Block { - BlockType Outport - Name "DPinv" - SID "107::5" - Position [460, 101, 480, 119] - ZOrder -6 - IconDisplay "Port number" - } - Line { - ZOrder 76 - SrcBlock "mat" - SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 - } - Line { - ZOrder 77 - SrcBlock "sigma" - SrcPort 1 - DstBlock " SFunction " - DstPort 2 - } - Line { - Name "DPinv" - ZOrder 78 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "DPinv" - DstPort 1 - } - Line { - ZOrder 79 - SrcBlock " Demux " - SrcPort 1 - DstBlock " Terminator " - DstPort 1 - } - Line { - ZOrder 80 - 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 [165, 310, 235, 340] - ZOrder 103 - BackgroundColor "yellow" - FunctionName "WBToolbox" - Parameters "'DiscreteFilter','WBToolboxLibrary',filterDataStruct" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 46 - $ClassName "Simulink.Mask" - Type "Discrete Filter" - Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." - Initialization "filterDataStruct = struct;\nfilterDataStruct.Fc = Fc;\nfilterDataStruct.Ts = Ts;\nfilterDataS" - "truct.MedianOrder = orderMedianFilter;\nfilterDataStruct.FilterType = filterType;\nfilterDataStruct.NumCoeffs = n" - "umCoeffs;\nfilterDataStruct.DenCoeffs = denCoeffs;\nfilterDataStruct.y0 = y0;\nfilterDataStruct.u0 = u0;\nfilterD" - "ataStruct.InitStatus = uint8(initStatus);" - Display "disp('Filter')" - RunInitForIconRedraw "off" - Array { - Type "Simulink.MaskParameter" - Dimension 9 - Object { - $ObjectID 47 - Type "popup" - Array { - Type "Cell" - Dimension 3 - Cell "Generic" - Cell "FirstOrderLowPassFilter" - Cell "MedianFilter" - PropName "TypeOptions" - } - Name "filterType" - Prompt "Type of the filter" - Value "Generic" + Name "controlType" + Prompt "Control Mode" + Value "Position" Evaluate "off" - 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 48 - Type "edit" - Name "numCoeffs" - Prompt "Numerator Coefficients*" - Value "[0]" - } - Object { - $ObjectID 49 - Type "edit" - Name "denCoeffs" - Prompt "Denominator Coefficients*" - Value "[0]" - } - Object { - $ObjectID 50 - Type "edit" - Name "Fc" - Prompt "Cut Frequency (Hz)" - Value "0" - Visible "off" - } - Object { - $ObjectID 51 - Type "edit" - Name "Ts" - Prompt "Sampling time (s)" - Value "0" - Visible "off" - } - Object { - $ObjectID 52 - Type "edit" - Name "orderMedianFilter" - Prompt "Order" - Value "0" - Visible "off" - } - Object { - $ObjectID 53 - 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;" + 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';'off'});\n case 'Position Direct'\n set_para" + "m(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off';'o" + "ff'});\n case 'Velocity'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n set" + "_param(gcb,'MaskVisibilities', {'on';'off';'on'});\n case 'Torque'\n set_param(gcb,'BackgroundColor', '[0." + "8275, 0.5765, 0.6039, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off';'off'});\n otherwise\n " + " error('Control Type not recognized.')\nend\n\nclear maskStr" } Object { - $ObjectID 54 + $ObjectID 55 Type "edit" - Name "y0" - Prompt "Output y0" - Value "[0]" - Visible "off" + Name "refSpeed" + Prompt "Reference Speed" + Value "10*(pi/180)" } Object { - $ObjectID 55 + $ObjectID 56 Type "edit" - Name "u0" - Prompt "Input u0" - Value "[0]" + Name "refAcceleration" + Prompt "Reference Velocity" + Value "1000000*(pi/180)" Visible "off" } PropName "Parameters" } Array { Type "Simulink.dialog.Group" - Dimension 2 + Dimension 3 Object { - $ObjectID 56 + $ObjectID 57 Prompt "%" Object { $PropName "DialogControls" - $ObjectID 57 + $ObjectID 58 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" @@ -1757,107 +1606,53 @@ Library { Name "DescGroupVar" } Object { - $ObjectID 58 + $ObjectID 59 Prompt "Simulink:studio:ToolBarParametersMenu" Array { - Type "Simulink.dialog.Control" - Dimension 8 - Object { - $ObjectID 59 - $ClassName "Simulink.dialog.parameter.Popup" - Name "filterType" - } + Type "Simulink.dialog.parameter.Control" + Dimension 3 Object { $ObjectID 60 - $ClassName "Simulink.dialog.parameter.Edit" - Name "numCoeffs" + $ClassName "Simulink.dialog.parameter.Popup" + Name "controlType" } Object { $ObjectID 61 $ClassName "Simulink.dialog.parameter.Edit" - Name "denCoeffs" - } - Object { - $ObjectID 62 - $ClassName "Simulink.dialog.Text" - Prompt "* The coefficients are ordered in increasing power of z^-1" - Name "howToCoeffs" - } - Object { - $ObjectID 63 - $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "Fc" - } - Object { - $ObjectID 64 - $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" - Name "Ts" + Name "refSpeed" } Object { - $ObjectID 65 + $ObjectID 62 $ClassName "Simulink.dialog.parameter.Edit" PromptLocation "left" - Name "orderMedianFilter" - } - Object { - $ObjectID 66 - $ClassName "Simulink.dialog.Group" - Array { - Type "Simulink.dialog.parameter.Control" - Dimension 3 - Object { - $ObjectID 67 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "initStatus" - } - Object { - $ObjectID 68 - $ClassName "Simulink.dialog.parameter.Edit" - Name "y0" - } - Object { - $ObjectID 69 - $ClassName "Simulink.dialog.parameter.Edit" - Name "u0" - } - PropName "DialogControls" - } - Name "Container3" + Name "refAcceleration" } PropName "DialogControls" } Name "ParameterGroupVar" } + Object { + $ObjectID 63 + Object { + $PropName "DialogControls" + $ObjectID 64 + $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" } } - } - Block { - BlockType SubSystem - Name "Match Signal Sizes" - SID "1818" - Ports [2, 1] - Position [440, 297, 525, 353] - ZOrder 251 - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 70 - $ClassName "Simulink.Mask" - Type "Match Signal Sizes" - Description "It can happen on particular cases that the Simulink engine cannot propagate the right sizes fro" - "m blocks of the toolbox. If you encounter such problems you can try using this blocks which introducing a minimal" - " overhead assigns to input Signal the same size of input Reference and outputs Signal.\n\nCurrently it supports o" - "nly 1D vectors." - Display "port_label('input', 1, 'Reference')\nport_label('input', 2, 'Signal')\nport_label('output', 1, 'Sig" - "nal')" - RunInitForIconRedraw "off" - } System { - Name "Match Signal Sizes" - Location [548, 183, 3416, 1872] + Name "SetReferences" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -1875,578 +1670,548 @@ Library { ZoomFactor "1000" Block { BlockType Inport - Name "Reference" - SID "1819" - Position [315, 178, 345, 192] - ZOrder 240 - NamePlacement "alternate" - IconDisplay "Port number" - } - Block { - BlockType Inport - Name "Signal" - SID "1820" - Position [315, 198, 345, 212] - ZOrder 241 - Port "2" + Name "References" + SID "1768" + Position [55, 48, 85, 62] + ZOrder 24 IconDisplay "Port number" } Block { - BlockType DotProduct - Name "Dot Product" - SID "1836" - Position [410, 177, 445, 213] - ZOrder 252 - OutDataTypeStr "Inherit: Inherit via internal rule" - } - Block { - BlockType Terminator - Name "Terminator" - SID "1822" - Position [485, 185, 505, 205] - ZOrder 245 - } - Block { - BlockType Outport - Name "Output" - SID "1823" - Position [410, 243, 440, 257] - ZOrder 244 - IconDisplay "Port number" - } - Line { - ZOrder 27 - SrcBlock "Dot Product" - SrcPort 1 - DstBlock "Terminator" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Signal" - SrcPort 1 - Points [17, 0] - Branch { - ZOrder 28 - Points [0, 45] - DstBlock "Output" - DstPort 1 - } - Branch { - ZOrder 26 - DstBlock "Dot Product" - DstPort 2 - } + BlockType S-Function + Name "S-Function" + SID "1769" + Ports [1] + Position [125, 39, 185, 71] + ZOrder 19 + FunctionName "BlockFactory" + Parameters "'SetReferences','WBToolbox',WBTConfigParameters,configBlockAbsName,controlType,refTraj" + "ectory" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off } Line { - ZOrder 24 - SrcBlock "Reference" + ZOrder 1 + SrcBlock "References" SrcPort 1 - DstBlock "Dot Product" + DstBlock "S-Function" DstPort 1 } } } + } + } + Block { + BlockType SubSystem + Name "Model" + SID "209" + Ports [] + Position [133, 16, 231, 113] + ZOrder -3 + ForegroundColor "white" + DropShadow on + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 65 + $ClassName "Simulink.Mask" + Display "image(imread('wholeBodyModel.png'),'center')" + } + System { + Name "Model" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "462" Block { - BlockType S-Function - Name "Minimum Jerk Trajectory Generator" - SID "1747" - Ports [1, 3] - Position [470, 93, 625, 147] - ZOrder 78 - FunctionName "WBToolbox" - Parameters "'MinimumJerkTrajectoryGenerator','WBToolboxLibrary',sampleTime,settlingTime,firstDerivatives,secondD" - "erivatives,explicitInitialValue,externalSettlingTime,resetOnSettlingTime" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off + BlockType SubSystem + Name "Dynamics" + SID "369" + Ports [] + Position [364, 21, 471, 128] + ZOrder -1 + DropShadow on + RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 71 + $ObjectID 66 $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 3rd order LTI dynamical system (for more details see [1]).\n\nPositi" - "on, velocity and acceleration trajectories are computed.\n\nThe main advantage with respect to the standard polyn" - "omial form is that if the reference value yd changes there is no need to recompute the filter coefficients.\n\n[1" - "]\nPattacini, U.; Nori, F.; Natale, L.; Metta, G.; Sandini, G.\n\"An experimental evaluation of a novel minimum-j" - "erk cartesian controller for humanoid robots\"\nIntelligent Robots and Systems (IROS), 2010\nIEEE/RSJ Internation" - "al Conference on , vol., no., pp.1668-1674, 18-22 Oct. 2010\ndoi: 10.1109/IROS.2010.5650851\nURL: http://ieeexplo" - "re.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" - RunInitForIconRedraw "on" - Array { - Type "Simulink.MaskParameter" - Dimension 7 - Object { - $ObjectID 72 - 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 73 - Type "edit" - Name "settlingTime" - Prompt "Settling Time" - Value "3" - } - Object { - $ObjectID 74 - Type "edit" - Name "sampleTime" - Prompt "Sample Time" - Value "0.01" - } - Object { - $ObjectID 75 - Type "checkbox" - Name "resetOnSettlingTime" - Prompt "Reset on Settling Time Changes" - Value "off" - Visible "off" - } - Object { - $ObjectID 76 - Type "checkbox" - Name "firstDerivatives" - Prompt "Output First Derivative" - Value "on" - } - Object { - $ObjectID 77 - Type "checkbox" - Name "secondDerivatives" - Prompt "Output Second Derivative" - Value "on" - } + Display "image(imread('Dynamics.png'))" + } + System { + Name "Dynamics" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "330" + Block { + BlockType SubSystem + Name "CentroidalMomentum" + SID "1694" + Ports [4, 1] + Position [475, 101, 660, 164] + ZOrder 72 + RequestExecContextInheritance off Object { - $ObjectID 78 - Type "checkbox" - Name "explicitInitialValue" - Prompt "Explicit Initial Value" - Value "off" - Visible "off" - } - PropName "Parameters" - } - Array { - Type "Simulink.dialog.Group" - Dimension 2 - Object { - $ObjectID 79 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 80 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 81 - Prompt "Simulink:studio:ToolBarParametersMenu" + $PropName "MaskObject" + $ObjectID 67 + $ClassName "Simulink.Mask" + Type "CentroidalMomentum" + Description "This block computed the 6 element centroidal momentum, as defined in:\n\n\"Centroidal dynamics of a hu" + "manoid robot\" - DE Orin, A Goswami, SH Lee\nAutonomous Robots 35 (2-3), 161-176\n\nAssuming DoFs is the number of i" + "nternal degrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transforma" + "tion between the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the con" + "figuration of the joints.\n - Base velocity: Vector of size 6 representing the linear and angular velocity of the b" + "ase frame.\n - Joints velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n - Centr" + "oidal Momentum: 6-element vector containg the centroidal momentum (3 value for linear momentum and 3 for angular mom" + "entum)." + 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 82 - $ClassName "Simulink.dialog.TabContainer" + $ObjectID 68 + $ClassName "Simulink.dialog.Group" + Prompt "%" Array { - Type "Simulink.dialog.Tab" + Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 83 - Prompt "Trajectory Parameters" - Array { - Type "Simulink.dialog.parameter.Control" - Dimension 4 - Object { - $ObjectID 84 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "externalSettlingTime" + $ObjectID 69 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } Object { - $ObjectID 85 - $ClassName "Simulink.dialog.parameter.Edit" - Name "settlingTime" - } + $ObjectID 70 + $ClassName "Simulink.dialog.Group" Object { - $ObjectID 86 - $ClassName "Simulink.dialog.parameter.Edit" - Name "sampleTime" + $PropName "DialogControls" + $ObjectID 71 + $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" } - Object { - $ObjectID 87 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "resetOnSettlingTime" + Name "ToggleButtonContainer" } PropName "DialogControls" - } - Name "Tab1" - } - Object { - $ObjectID 88 - Prompt "Input/Output" - Array { - Type "Simulink.dialog.parameter.CheckBox" - Dimension 3 + } + Name "DescGroupVar" + } + } + System { + Name "CentroidalMomentum" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "480" + 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 "BlockFactory" + Parameters "'CentroidalMomentum','WBToolbox',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "CentroidalMomentum" + SID "1700" + Position [315, 73, 345, 87] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "CentroidalMomentum" + 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 "GetBiasForces" + SID "1724" + Ports [4, 1] + Position [400, 212, 540, 313] + ZOrder 73 + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 72 + $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 intern" + "al degrees 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 configur" + "ation of the joints.\n - Base velocity: Vector of size 6 representing the linear and angular velocity of the base f" + "rame.\n - Joints velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n - Bias Force" + "s: a Dofs + 6 vector 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 "off" + Object { + $PropName "DialogControls" + $ObjectID 73 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 Object { - $ObjectID 89 - Name "firstDerivatives" + $ObjectID 74 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } Object { - $ObjectID 90 - Name "secondDerivatives" - } + $ObjectID 75 + $ClassName "Simulink.dialog.Group" Object { - $ObjectID 91 - Name "explicitInitialValue" - } - PropName "DialogControls" + $PropName "DialogControls" + $ObjectID 76 + $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 "Tab2" + Name "ToggleButtonContainer" } PropName "DialogControls" } - Name "Container3" + Name "DescGroupVar" } - Name "ParameterGroupVar" } - PropName "DialogControls" - } - } - } - Block { - BlockType S-Function - Name "QP" - SID "1807" - Ports [4, 2] - Position [405, -6, 595, 66] - ZOrder 104 - FunctionName "WBToolbox" - Parameters "'QpOases','WBToolboxLibrary',lbA,ubA,lb,ub,computeObjVal,stopIfFails" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 92 - $ClassName "Simulink.Mask" - Type "QP" - Description "This block solves a QP problem using the qpOASES library:\n\nx = argmin 0.5 * x' * H * x + c' *" - " x\n\ns.t.\n\nlbA <= Ax <= ubA\nlb <= x <= ub \n" - Display "disp('QP')\n\nport_label('output', 1, 'PrimalSolution')\nport_label('output', 2, 'Status')\n\nif (c" - "omputeObjVal)\n port_label('output', 3, 'Obj func value')\nend\n\nport_label('input', 1, 'H');\nport_label('in" - "put', 2, 'g');\n\ninputPortNumber = 2;\n\nif (lbA || ubA)\n inputPortNumber = 3;\n port_label('input', 3, '" - "A');\nend\n\nif (lbA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lbA'" - ");\nend\nif (ubA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ubA');\n" - "end\nif (lb)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lb');\nend\ni" - "f (ub)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ub');\nend" - RunInitForIconRedraw "on" - Array { - Type "Simulink.MaskParameter" - Dimension 6 - Object { - $ObjectID 93 - Type "checkbox" - Name "lbA" - Prompt "Accept constraints lower bound input (lbA)" - Value "off" - } - Object { - $ObjectID 94 - Type "checkbox" - Name "ubA" - Prompt "Accept constraints upper bound input (ubA)" - Value "on" - } - Object { - $ObjectID 95 - Type "checkbox" - Name "lb" - Prompt "Accept lower bound input (lb)" - Value "off" - } - Object { - $ObjectID 96 - Type "checkbox" - Name "ub" - Prompt "Accept upper bound input (ub)" - Value "off" - } - Object { - $ObjectID 97 - Type "checkbox" - Name "computeObjVal" - Prompt "Output the value of the objective function" - Value "off" - } - Object { - $ObjectID 98 - Type "checkbox" - Name "stopIfFails" - Prompt "Stop the simulation if the solver fails" - Value "off" + System { + Name "GetBiasForces" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "395" + 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 "BlockFactory" + Parameters "'InverseDynamics','WBToolbox',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 + } } - PropName "Parameters" } - Array { - Type "Simulink.dialog.Group" - Dimension 2 + Block { + BlockType SubSystem + Name "GetGravityForces" + SID "1733" + Ports [2, 1] + Position [600, 210, 740, 310] + ZOrder 74 + RequestExecContextInheritance off Object { - $ObjectID 99 - Prompt "%" + $PropName "MaskObject" + $ObjectID 77 + $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 in" + "ternal degrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformat" + "ion between the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the conf" + "iguration 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 100 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 101 - Prompt "Simulink:studio:ToolBarParametersMenu" - Array { - Type "Simulink.dialog.Group" - Dimension 3 - Object { - $ObjectID 102 - Prompt "Constraints bounds" - Array { - Type "Simulink.dialog.parameter.CheckBox" + $ObjectID 78 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" Dimension 2 Object { - $ObjectID 103 - Name "lbA" + $ObjectID 79 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } Object { - $ObjectID 104 - Name "ubA" - } - PropName "DialogControls" - } - Name "constraintsBoundsBox" - } - Object { - $ObjectID 105 - Prompt "Simple bounds" - Array { - Type "Simulink.dialog.parameter.CheckBox" - Dimension 2 - Object { - $ObjectID 106 - Name "lb" - } - Object { - $ObjectID 107 - Name "ub" - } - PropName "DialogControls" - } - Name "simpleBoundsBox" - } - Object { - $ObjectID 108 - Prompt "Other" - Array { - Type "Simulink.dialog.parameter.CheckBox" - Dimension 2 + $ObjectID 80 + $ClassName "Simulink.dialog.Group" Object { - $ObjectID 109 - Name "computeObjVal" + $PropName "DialogControls" + $ObjectID 81 + $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" } - Object { - $ObjectID 110 - Name "stopIfFails" + Name "ToggleButtonContainer" } PropName "DialogControls" - } - Name "otherBox" } - PropName "DialogControls" + Name "DescGroupVar" } - Name "ParameterGroupVar" - } - PropName "DialogControls" - } - } - } - Block { - BlockType S-Function - Name "Real Time Synchronizer" - SID "1657" - Ports [] - Position [70, 4, 195, 41] - ZOrder 23 - ForegroundColor "[0.917647, 0.917647, 0.917647]" - BackgroundColor "gray" - ShowName off - FunctionName "WBToolbox" - Parameters "'RealTimeSynchronizer','WBToolboxLibrary',period" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 111 - $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')" - RunInitForIconRedraw "off" - Object { - $PropName "Parameters" - $ObjectID 112 - $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 [230, 4, 360, 41] - ZOrder 24 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - ShowName off - FunctionName "WBToolbox" - Parameters "'SimulatorSynchronizer','WBToolboxLibrary',period, serverPortName, clientPortName" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 113 - $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')" - RunInitForIconRedraw "off" - Array { - Type "Simulink.MaskParameter" - Dimension 3 - Object { - $ObjectID 114 - Type "edit" - Name "period" - Prompt "Controller Period (in seconds)" - Value "0.01" - } - Object { - $ObjectID 115 - Type "edit" - Name "serverPortName" - Prompt "Server Port Name" - Value "'/clock/rpc'" - } - Object { - $ObjectID 116 - 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 [375, 198, 445, 242] - ZOrder -3 - BackgroundColor "[0.534601, 0.470279, 1.000000]" - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 117 - $ClassName "Simulink.Mask" - RunInitForIconRedraw "off" - Object { - $PropName "Parameters" - $ObjectID 118 - $ClassName "Simulink.MaskParameter" - Type "edit" - Name "tol" - Prompt "Tolerance" - Value "1e-4" - } - } - System { - Name "TruncPinv" - Location [0, 29, 1280, 744] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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" System { - Name "Truncated PseudoInverse" - Location [12, 45, 1279, 3773] + Name "GetGravityForces" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -2461,341 +2226,358 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "1619" + ZoomFactor "395" Block { BlockType Inport - Name "mat" - SID "112::1" - Position [20, 101, 40, 119] - ZOrder -1 + Name "Base Pose" + SID "1784" + Position [10, 38, 40, 52] + ZOrder 22 IconDisplay "Port number" } Block { BlockType Inport - Name "tol" - SID "112::25" - Position [20, 136, 40, 154] - ZOrder 11 + Name "Joint configuration" + SID "1785" + Position [10, 68, 40, 82] + ZOrder 24 Port "2" IconDisplay "Port number" } Block { - BlockType Demux - Name " Demux " - SID "112::1618" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 107 - Outputs "1" + 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 " SFunction " - SID "112::1617" - Tag "Stateflow S-Function WBToolboxLibrary_repository 7" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 106 - FunctionName "sf_sfun" - PortCounts "[2 2]" + Name "S-Function" + SID "1788" + Ports [6, 1] + Position [180, 24, 240, 216] + ZOrder 19 + FunctionName "BlockFactory" + Parameters "'InverseDynamics','WBToolbox',WBTConfigParameters,configBlockAbsName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "TPinv" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "112::1619" - Position [460, 241, 480, 259] - ZOrder 108 } Block { BlockType Outport - Name "TPinv" - SID "112::5" - Position [460, 101, 480, 119] - ZOrder -6 + Name "Gravity Torques" + SID "1789" + Position [300, 113, 330, 127] + ZOrder 25 IconDisplay "Port number" } Line { - ZOrder 76 - SrcBlock "mat" + ZOrder 1 + SrcBlock "S-Function" SrcPort 1 - Points [120, 0] - DstBlock " SFunction " + DstBlock "Gravity Torques" DstPort 1 } Line { - ZOrder 77 - SrcBlock "tol" + ZOrder 2 + SrcBlock "Base Pose" SrcPort 1 - DstBlock " SFunction " - DstPort 2 + DstBlock "S-Function" + DstPort 1 } Line { - Name "TPinv" - ZOrder 78 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "TPinv" - DstPort 1 + 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 79 - SrcBlock " Demux " + ZOrder 8 + SrcBlock "Constant" SrcPort 1 - DstBlock " Terminator " - DstPort 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 80 - SrcBlock " SFunction " + ZOrder 13 + SrcBlock "Gain" SrcPort 1 - DstBlock " Demux " - DstPort 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 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, 96, 405, 144] - ZOrder 85 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpClock','WBToolboxLibrary'" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 119 - $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" - RunInitForIconRedraw "off" - } - } - Block { - BlockType S-Function - Name "Yarp Read" - SID "1632" - Ports [0, 2] - Position [90, 94, 150, 141] - ZOrder 22 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpRead','WBToolboxLibrary',portName,signalSize,blocking,timestamp,autoconnect,errorOnConnection,t" - "imeout" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 120 - $ClassName "Simulink.Mask" - Type "YARP Read" - Description "This block behaves as the command 'yarp read'. \n\nBy default it has the option 'Autoconnect' a" - "ctive, which means that the user can only specify the name of the port ('Source Port Name') from which readings a" - "re desired, along with the size of the expected data (e.g. 3 when reading the torso state).\n\nWhen the user unch" - "ecks 'Autoconnect', 'Opened Port Name' field shows up and it corresponds to the name of the port the block will c" - "reate. No autoconnection is performed, and the output will be always zero until the user connects it to some othe" - "r port from command line using 'yarp connect [from] [dest]'.\n\nWhen 'Autoconnect' is off, an additional output c" - "alled 'signal' is added to the block. It outputs 1 if the port has received at least one sample, or 0 otherwise.\n" - Initialization "% Solve error in string parsing. Force the type.\nportName = char(portName);" - 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" - RunInitForIconRedraw "on" - Array { - Type "Simulink.MaskParameter" - Dimension 7 - Object { - $ObjectID 121 - Type "edit" - Name "portName" - Prompt "Source Port Name" - Value "'/portname'" - } - Object { - $ObjectID 122 - Type "edit" - Name "signalSize" - Prompt "Port Size" - Value "1" - } + BlockType S-Function + Name "InverseDynamics" + SID "1748" + Ports [6, 1] + Position [190, 199, 355, 331] + ZOrder 75 + FunctionName "BlockFactory" + Parameters "'InverseDynamics','WBToolbox',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off Object { - $ObjectID 123 - Type "edit" - Name "timeout" - Prompt "Timeout" - Value "0.5" - Visible "off" + $PropName "MaskObject" + $ObjectID 82 + $ClassName "Simulink.Mask" + Type "InverseDynamics" + Description "This block compute the inverse dynamics of the robot.\n\nAssuming DoFs is the number of internal degre" + "es 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 - 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 corre" + "sponding torques required 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 "off" + Object { + $PropName "DialogControls" + $ObjectID 83 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 84 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 85 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 86 + $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 "MassMatrix" + SID "1633" + Ports [2, 1] + Position [250, 104, 390, 171] + ZOrder 32 + RequestExecContextInheritance off Object { - $ObjectID 124 - Type "checkbox" - Name "blocking" - Prompt "Blocking Read" - Value "off" - Callback "blocking_val = get_param(gcb, 'blocking');\nmask_visibility = get_param(gcb, 'MaskVisibilities');\n\nif(s" - "trcmp(blocking_val, 'on'))\n mask_visibility{3} = 'on';\n set_param(gcb, 'MaskVisibilities', mask_visibility);" - "\nelse\n mask_visibility{3} = 'off';\n set_param(gcb, 'MaskVisibilities', mask_visibility);\nend\n\nclear bloc" - "king_val mask_visibility" - } - Object { - $ObjectID 125 - Type "checkbox" - Name "timestamp" - Prompt "Read Timestamp" - Value "on" - } - Object { - $ObjectID 126 - Type "checkbox" - Name "autoconnect" - Prompt "Autoconnect" - Value "on" - Callback "autoconnect_val = get_param(gcb, 'Autoconnect');\nprompt_string = get_param(gcb, 'MaskPrompts');\nmask_vi" - "sibilities = get_param(gcb, 'MaskVisibilities');\n\nif(strcmp(autoconnect_val, 'on'))\n prompt_string{1} = 'Sourc" - "e Port Name';\n mask_visibilities{7} = 'on';\n set_param(gcb, 'MaskVisibilities',mask_visibilities);\nelse\n " - " prompt_string{1} = 'Opened Port Name';\n mask_visibilities{7} = 'off';\n set_param(gcb, 'MaskVisibilities',m" - "ask_visibilities);\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string mask_vis" - "ibilities" - } - Object { - $ObjectID 127 - 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, 96, 290, 144] - ZOrder 27 - ForegroundColor "[0.490196, 0.000000, 0.000000]" - FunctionName "WBToolbox" - Parameters "'YarpWrite','WBToolboxLibrary',portName,autoconnect,errorOnConnection" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 128 - $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')" - RunInitForIconRedraw "off" - Array { - Type "Simulink.MaskParameter" - Dimension 3 - Object { - $ObjectID 129 - Type "edit" - Name "portName" - Prompt "Opened Port Name" - Value "'/portname'" - } - Object { - $ObjectID 130 - 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" + $PropName "MaskObject" + $ObjectID 87 + $ClassName "Simulink.Mask" + Type "MassMatrix" + Description "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of internal degrees of free" + "dom of the robot,\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformation between the base f" + "rame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the configuration of the joint" + "s.\n\nOutput:\n - MassMatrix: 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 "off" + Object { + $PropName "DialogControls" + $ObjectID 88 + $ClassName "Simulink.dialog.Group" + Prompt "%" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 89 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Object { + $ObjectID 90 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 91 + $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" + } } - Object { - $ObjectID 131 - Type "checkbox" - Name "errorOnConnection" - Prompt "Error on missing connection" - Value "on" - Visible "off" + System { + Name "MassMatrix" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "630" + 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 "BlockFactory" + Parameters "'MassMatrix','WBToolbox',WBTConfigParameters,configBlockAbsName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "MassMatrix" + SID "1637" + Position [245, 48, 275, 62] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "MassMatrix" + 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 + } } - PropName "Parameters" } } } Block { BlockType SubSystem - Name "errors" - SID "714" - Ports [2, 2] - Position [245, 198, 305, 242] - ZOrder -9 - BackgroundColor "[0.300000, 0.580000, 1.000000]" + Name "Jacobians" + SID "202" + Ports [] + Position [217, 20, 324, 127] + ZOrder -3 + DropShadow on RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 132 + $ObjectID 92 $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." - RunInitForIconRedraw "off" + Display "image(imread('jacobian.png'))" } System { - Name "errors" - Location [0, 29, 1280, 744] + Name "Jacobians" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -2810,201 +2592,97 @@ Library { 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 [115, 197, 175, 243] - ZOrder 14 - BackgroundColor "[0.537255, 0.721569, 1.000000]" - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 133 - $ClassName "Simulink.Mask" - Type "Holder" - Description "This block holds the first input value during the simulation." - RunInitForIconRedraw "off" - } - System { - Name "holder\n" - Location [12, 45, 1340, 980] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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.441" - 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 - } + ZoomFactor "480" 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 + Name "DotJNu" + SID "1683" + Ports [4, 1] + Position [590, 170, 755, 265] + ZOrder 67 RequestExecContextInheritance off - SFBlockType "MATLAB Function" + Object { + $PropName "MaskObject" + $ObjectID 93 + $ClassName "Simulink.Mask" + Type "DotJNu" + 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 internal degrees of freedom of the r" + "obot:\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 - " + "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 6D vector representing th" + "e product between the time derivative of the Jacobian of the specified frame and the state velocity vector\n\nParame" + "ters:\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 94 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 95 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 96 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 97 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 98 + $ClassName "Simulink.dialog.parameter.Edit" + Name "frameName" + } + Object { + $ObjectID 99 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 100 + $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 "MATLAB Function" - Location [12, 45, 1135, 3068] + Name "DotJNu" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -3019,496 +2697,285 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "100" - SIDHighWatermark "1625" + ZoomFactor "595" Block { BlockType Inport - Name "s" - SID "1300::24" - Position [20, 101, 40, 119] - ZOrder 10 + Name "Base Pose" + SID "1684" + Position [20, 13, 50, 27] + ZOrder 22 IconDisplay "Port number" } Block { BlockType Inport - Name "unused" - SID "1300::26" - Position [20, 136, 40, 154] - ZOrder 12 + Name "Joint configuration" + SID "1685" + Position [20, 43, 50, 57] + ZOrder 24 Port "2" IconDisplay "Port number" } Block { - BlockType Demux - Name " Demux " - SID "1300::1624" - Ports [1, 1] - Position [270, 230, 320, 270] - ZOrder 95 - Outputs "1" + 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 " SFunction " - SID "1300::1623" - Tag "Stateflow S-Function WBToolboxLibrary_repository 1" - Ports [2, 2] - Position [180, 100, 230, 160] - ZOrder 94 - FunctionName "sf_sfun" - PortCounts "[2 2]" + Name "S-Function" + SID "1688" + Ports [4, 1] + Position [125, 4, 190, 126] + ZOrder 19 + FunctionName "BlockFactory" + Parameters "'DotJNu','WBToolbox',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off - Port { - PortNumber 2 - Name "s0" - } - } - Block { - BlockType Terminator - Name " Terminator " - SID "1300::1625" - Position [460, 241, 480, 259] - ZOrder 96 } Block { BlockType Outport - Name "s0" - SID "1300::25" - Position [460, 101, 480, 119] - ZOrder 11 + Name "dotJ dotnu" + SID "1689" + Position [245, 58, 275, 72] + ZOrder 25 IconDisplay "Port number" } Line { - ZOrder 76 - SrcBlock "s" + ZOrder 1 + SrcBlock "Joint configuration" SrcPort 1 - Points [120, 0] - DstBlock " SFunction " - DstPort 1 + DstBlock "S-Function" + DstPort 2 } Line { - ZOrder 77 - SrcBlock "unused" + ZOrder 2 + SrcBlock "Base Pose" SrcPort 1 - DstBlock " SFunction " - DstPort 2 + DstBlock "S-Function" + DstPort 1 } Line { - Name "s0" - ZOrder 78 - Labels [0, 0] - SrcBlock " SFunction " - SrcPort 2 - DstBlock "s0" + ZOrder 3 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "dotJ dotnu" DstPort 1 } Line { - ZOrder 79 - SrcBlock " Demux " + ZOrder 4 + SrcBlock "Base velocity" SrcPort 1 - DstBlock " Terminator " - DstPort 1 + DstBlock "S-Function" + DstPort 3 } Line { - ZOrder 80 - SrcBlock " SFunction " + ZOrder 5 + SrcBlock "Joints velocity" SrcPort 1 - DstBlock " Demux " - DstPort 1 + DstBlock "S-Function" + DstPort 4 } } } 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 - Object { - $PropName "MaskObject" - $ObjectID 134 - $ClassName "Simulink.Mask" - Display "image(imread('wholeBodyActuators.png'),'center')" - } - System { - Name "wholeBodyActuators" - Location [832, 225, 3700, 1914] - Open on - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "1000" - Block { - BlockType S-Function - Name "Set Motor Parameters" - SID "1847" - Ports [1, 1] - Position [645, 290, 755, 340] - ZOrder 106 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - FunctionName "WBToolbox" - Parameters "'SetMotorParameters','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName,readP,readI,readD,pid" - "Type,setKTau,kTau,setBemf,bemf" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - Object { - $PropName "MaskObject" - $ObjectID 135 - $ClassName "Simulink.Mask" - Type "Set Motor Parameters" - Description "This block allows to set motor PIDs and constants such as the motor constant and the back EMF.\n" - "\nThe motor constant and the back EMF are set in the block's mask, and their values are applied during the first " - "simulation loop. The PIDs values are instead set from the input ports.\n\nAll these vectors should have a size eq" - "ual to the number of controlled joints specified in the referred Configuration block.\n\nAt the end of the simula" - "tion, the default values are restored." - 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% The booleans of the checkboxes always create problems\nreadP = uint8(readP);" - "\nreadI = uint8(readI);\nreadD = uint8(readD);" - Display "fprintf('Motor Parameters\\n(%s)',pidType);\n\nnumPorts = 0;\n\nif readP\n numPorts = numPorts +" - " 1;\n port_label('input',numPorts,'P')\nend\n\nif readI\n numPorts = numPorts + 1;\n port_label('input'," - "numPorts,'I')\nend\n\nif readD\n numPorts = numPorts + 1;\n port_label('input',numPorts,'D')\nend\n" - RunInitForIconRedraw "on" - Array { - Type "Simulink.MaskParameter" - Dimension 8 + BlockType SubSystem + Name "Jacobian" + SID "1663" + Ports [2, 1] + Position [380, 190, 540, 245] + ZOrder 39 + RequestExecContextInheritance off Object { - $ObjectID 136 - Type "popup" + $PropName "MaskObject" + $ObjectID 101 + $ClassName "Simulink.Mask" + Type "Jacobian" + Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of internal d" + "egrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformation betw" + "een the the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the configur" + "ation of the joints.\n\nOutput:\n - Jacobian: a 6x6+dofs matrix representing the Jacobian of the specified frame wr" + "itten in 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} 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 102 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" + } Array { - Type "Cell" + Type "Simulink.dialog.Group" 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" - } - Object { - $ObjectID 137 - Type "checkbox" - Name "readP" - Prompt "Enable P gains input signal" - Value "on" - } - Object { - $ObjectID 138 - Type "checkbox" - Name "readI" - Prompt "Enable I gains input signal" - Value "off" - } - Object { - $ObjectID 139 - Type "checkbox" - Name "readD" - Prompt "Enable D gains input signal" - Value "off" - } - Object { - $ObjectID 140 - Type "checkbox" - Name "setKTau" - Prompt "Set motor constant KTau" - Value "off" - Callback "h = Simulink.Mask.get(gcb);\nsetKTauValue = get_param(gcb,'setKTau');\n\nif ~strcmp(setKTauValue,'on')\n " - " h.Parameters(6).Enabled = 'off';\nelse\n h.Parameters(6).Enabled = 'on';\nend\n\nclear h setKTauValue" - } - Object { - $ObjectID 141 - Type "edit" - Name "kTau" - Prompt "kTau" - Value "[0]" - Enabled "off" - } - Object { - $ObjectID 142 - Type "checkbox" - Name "setBemf" - Prompt "Set motor back EMF constant" - Value "off" - Callback "h = Simulink.Mask.get(gcb);\nsetBemfValue = get_param(gcb,'setBemf');\n\nif ~strcmp(setBemfValue,'on')\n " - " h.Parameters(8).Enabled = 'off';\nelse\n h.Parameters(8).Enabled = 'on';\nend\n\nclear h setBemfValue" - } - Object { - $ObjectID 143 - Type "edit" - Name "bemf" - Prompt "bemf" - Value "[0]" - Enabled "off" - } - PropName "Parameters" - } - Array { - Type "Simulink.dialog.Group" - Dimension 2 - Object { - $ObjectID 144 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 145 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 146 - Prompt "Simulink:studio:ToolBarParametersMenu" - Array { - Type "Simulink.dialog.Group" - Dimension 3 Object { - $ObjectID 147 - Prompt "Gains" - Array { - Type "Simulink.dialog.parameter.Control" - Dimension 4 - Object { - $ObjectID 148 - $ClassName "Simulink.dialog.parameter.Popup" - Name "pidType" - } - Object { - $ObjectID 149 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "readP" - } - Object { - $ObjectID 150 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "readI" - } + $ObjectID 103 + Prompt "%" Object { - $ObjectID 151 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "readD" - } - PropName "DialogControls" + $PropName "DialogControls" + $ObjectID 104 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } - Name "Container3" + Name "DescGroupVar" } Object { - $ObjectID 152 - Prompt "Motor Torque Parameters" + $ObjectID 105 + Prompt "Simulink:studio:ToolBarParametersMenu" Array { - Type "Simulink.dialog.parameter.Control" - Dimension 4 - Object { - $ObjectID 153 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "setKTau" - } + Type "Simulink.dialog.Control" + Dimension 2 Object { - $ObjectID 154 + $ObjectID 106 $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "kTau" - } - Object { - $ObjectID 155 - $ClassName "Simulink.dialog.parameter.CheckBox" - Name "setBemf" + Name "frameName" } Object { - $ObjectID 156 - $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "bemf" - } - PropName "DialogControls" - } - Name "Container4" - } - Object { - $ObjectID 157 + $ObjectID 107 + $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 158 + $ObjectID 108 $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 "Control2" + Name "toggleHighlighting" } Name "ToggleButtonContainer" + } + PropName "DialogControls" + } + Name "ParameterGroupVar" } PropName "DialogControls" } - Name "ParameterGroupVar" } - PropName "DialogControls" + System { + Name "Jacobian" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "595" + 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 "BlockFactory" + Parameters "'Jacobian','WBToolbox',WBTConfigParameters,configBlockAbsName,frameName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "ForwardKinematics" + SID "1667" + Position [245, 48, 275, 62] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "ForwardKinematics" + 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 "Set Motor References" - SID "1791" - Ports [1] - Position [710, 210, 785, 260] - ZOrder 44 - BackgroundColor "[0.960000, 0.770000, 0.460000]" + Name "Kinematics" + SID "176" + Ports [] + Position [70, 20, 177, 127] + ZOrder -17 + DropShadow on RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 159 + $ObjectID 109 $ClassName "Simulink.Mask" - Type "Set Motor References" - Description "This block sets motor references. The type of control mode is specified as a parameter in the b" - "lock.\n\nAssuming N as the number of motors (usually equal to internal DoFs):\n\nInput:\n - References: Vector o" - "f size N, representing the references to be sent to the robot controlled motors.\n\nParameters:\n - Control Mode" - ": The control mode. Choose one of the supplied mode.\n\nUnit of measurement:\n - PWM [-100, 100]% Duty cycle\n " - " - Current [A]\n - Back EMF\n - Torque Constant" - Initialization "refSpeed = 0;\n\n% WBBlock Initialization\n% ======================\n\ntry\n [configBlockA" - "bsName, WBTConfig] = WBToolbox.BlockInitialization(gcb, gcs);\n \n WBTConfigParameters = WBTConfig.getSimul" - "inkParameters;\n clear WBTConfig;\ncatch\nend" - Display "disp(get_param(gcb,'controlType'))\n%port_label('input',1,'References')" - RunInitForIconRedraw "off" - Object { - $PropName "Parameters" - $ObjectID 160 - $ClassName "Simulink.MaskParameter" - Type "popup" - Array { - Type "Cell" - Dimension 4 - Cell "PWM" - Cell "Current" - Cell "Back EMF" - Cell "Torque Constant" - PropName "TypeOptions" - } - Name "controlType" - Prompt "Control Mode" - Value "Current" - Evaluate "off" - Callback "% Get the mask parameter values. This is a cell\n% array of strings.\n\nmaskStr = get_param(" - "gcb,'controlType');\n\nswitch maskStr\n case 'PWM'\n set_param(gcb,'BackgroundColor', '[1, 1, 1, 1.0]" - "');\n case 'Current'\n set_param(gcb,'BackgroundColor', '[0.96, 0.77, 0.46, 1.0]');\n case 'Back E" - "MF'\n set_param(gcb,'BackgroundColor', '[0.8, 0.4, 1.0, 1.0]');\n case 'Torque Constant'\n set" - "_param(gcb,'BackgroundColor', '[0.8, 1.0, 0.4, 1.0]');\n otherwise\n error('Control Type not recogniz" - "ed.')\nend\n\nclear maskStr" - } - Array { - Type "Simulink.dialog.Group" - Dimension 3 - Object { - $ObjectID 161 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 162 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 163 - Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 164 - $ClassName "Simulink.dialog.parameter.Popup" - Name "controlType" - } - Name "ParameterGroupVar" - } - Object { - $ObjectID 165 - Object { - $PropName "DialogControls" - $ObjectID 166 - $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" - } + Display "image(imread('forwardKinematics.png'))" } System { - Name "Set Motor References" - Location [832, 225, 3700, 1914] + Name "Kinematics" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -3523,419 +2990,144 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "1000" - Block { - BlockType Inport - Name "References" - SID "1794" - Position [55, 48, 85, 62] - ZOrder 24 - IconDisplay "Port number" - } + ZoomFactor "469" Block { - BlockType S-Function - Name "S-Function" - SID "1795" - Ports [1] - Position [125, 39, 185, 71] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'SetReferences','WBToolboxLibrary',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 "Set References" - SID "1767" - Ports [1] - Position [605, 210, 680, 260] - ZOrder 43 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 167 - $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 internal degrees of freedom configured in the asso" - "ciated Config block:\n\nInput:\n - References: Vector of size DoFs, representing the references to be sent to th" - "e robot controlled joints.\n - Reference Speed (Position): Set the reference speed in rad/s used by the trajecto" - "ry generator of the IPositionControl interface\n - Reference Acceleration (Velocity): Set the reference speed in" - " rad/s/s used by the trajectory generator of the IVelocityControl interface\n\nParameters:\n - Control Mode: The" - " control mode. Choose one of the supplied mode.\n\nUnit of measurement:\n- Position [rad]\n- Velocity [rad/s]\n- " - "Acceleration [rad/s/s]\n- Torque [Nm]" - Initialization "if (strcmp(controlType,'Position'))\n refTrajectory = refSpeed;\nelseif (strcmp(controlTyp" - "e,'Velocity'))\n refTrajectory = refAcceleration;\nelse\n refTrajectory = 0;\nend\n\n% WBBlock Initializati" - "on\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 3 - Object { - $ObjectID 168 - Type "popup" - Array { - Type "Cell" - Dimension 4 - Cell "Position" - Cell "Position Direct" - Cell "Velocity" - Cell "Torque" - 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';'off'});\n case 'Position Direct'\n set_para" - "m(gcb,'BackgroundColor', '[0.3922, 0.7882, 0.3451, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off';'o" - "ff'});\n case 'Velocity'\n set_param(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n set" - "_param(gcb,'MaskVisibilities', {'on';'off';'on'});\n case 'Torque'\n set_param(gcb,'BackgroundColor', '[0." - "8275, 0.5765, 0.6039, 1.0]');\n set_param(gcb,'MaskVisibilities', {'on';'off';'off'});\n otherwise\n " - " error('Control Type not recognized.')\nend\n\nclear maskStr" - } - Object { - $ObjectID 169 - Type "edit" - Name "refSpeed" - Prompt "Reference Speed" - Value "10*(pi/180)" - } - Object { - $ObjectID 170 - Type "edit" - Name "refAcceleration" - Prompt "Reference Velocity" - Value "1000000*(pi/180)" - Visible "off" - } - PropName "Parameters" - } - Array { - Type "Simulink.dialog.Group" - Dimension 3 + BlockType SubSystem + Name "ForwardKinematics" + SID "1647" + Ports [2, 1] + Position [360, 103, 510, 167] + ZOrder 34 + RequestExecContextInheritance off Object { - $ObjectID 171 - Prompt "%" + $PropName "MaskObject" + $ObjectID 110 + $ClassName "Simulink.Mask" + Type "ForwardKinematics" + Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " + "internal degrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transform" + "ation between the the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing th" + "e configuration of the joints.\n\nOutput:\n - ForwardKinematics: a 4x4 matrix representing the homogenous transfor" + "mation between the specified frame and the world frame.\n\nParameters:\n - Frame name: the name of the frame. It sh" + "ould 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, '_', '\\_');\n% port_label('output', 1, strcat('{}^{world} H_{',esca" + "pedFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('inp" + "ut', 2, 'Joint configuration')\n\nclear escapedFrameName;" + RunInitForIconRedraw "on" Object { - $PropName "DialogControls" - $ObjectID 172 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" + $PropName "Parameters" + $ObjectID 111 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "frameName" + Prompt "Frame name" + Value "'frame'" } - Name "DescGroupVar" - } - Object { - $ObjectID 173 - Prompt "Simulink:studio:ToolBarParametersMenu" Array { - Type "Simulink.dialog.parameter.Control" - Dimension 3 - Object { - $ObjectID 174 - $ClassName "Simulink.dialog.parameter.Popup" - Name "controlType" - } + Type "Simulink.dialog.Group" + Dimension 2 Object { - $ObjectID 175 - $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "refSpeed" + $ObjectID 112 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 113 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" } Object { - $ObjectID 176 + $ObjectID 114 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 2 + Object { + $ObjectID 115 $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "refAcceleration" + Name "frameName" + } + Object { + $ObjectID 116 + $ClassName "Simulink.dialog.Group" + Object { + $PropName "DialogControls" + $ObjectID 117 + $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" } - Name "ParameterGroupVar" } - Object { - $ObjectID 177 - 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(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 [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "1000" - 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','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName,controlType,refTraj" - "ectory" - 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 - Object { - $PropName "MaskObject" - $ObjectID 179 - $ClassName "Simulink.Mask" - Display "image(imread('wholeBodyModel.png'),'center')" - } - System { - Name "wholeBodyModel" - Location [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "551" - Block { - BlockType SubSystem - Name "Dynamics" - SID "369" - Ports [] - Position [364, 21, 471, 128] - ZOrder -1 - DropShadow on - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 180 - $ClassName "Simulink.Mask" - Display "image(imread('Dynamics.png'))" - } - System { - Name "Dynamics" - Location [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "500" - Block { - BlockType SubSystem - Name "Centroidal Momentum" - SID "1694" - Ports [4, 1] - Position [475, 101, 660, 164] - ZOrder 72 - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 181 - $ClassName "Simulink.Mask" - Type "Centroidal Momentum" - Description "This block computed the 6 element centroidal momentum, as defined in:\n\n\"Centroidal dynamics of a hu" - "manoid robot\" - DE Orin, A Goswami, SH Lee\nAutonomous Robots 35 (2-3), 161-176\n\nAssuming DoFs is the number of i" - "nternal degrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transforma" - "tion between the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the con" - "figuration of the joints.\n - Base velocity: Vector of size 6 representing the linear and angular velocity of the b" - "ase frame.\n - Joints velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n - Centr" - "oidal Momentum: 6-element vector containg the centroidal momentum (3 value for linear momentum and 3 for angular mom" - "entum)." - 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 182 - $ClassName "Simulink.dialog.Group" - Prompt "%" - Array { - Type "Simulink.dialog.Control" - Dimension 2 - Object { - $ObjectID 183 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Object { - $ObjectID 184 - $ClassName "Simulink.dialog.Group" - Object { - $PropName "DialogControls" - $ObjectID 185 - $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 [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "725" - Block { - BlockType Inport - Name "Base Pose" - SID "1695" - Position [20, 18, 50, 32] - ZOrder 22 - IconDisplay "Port number" + System { + Name "ForwardKinematics" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "595" + 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 "1696" - Position [20, 53, 50, 67] + SID "1649" + Position [20, 63, 50, 77] 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] + SID "1650" + Ports [2, 1] + Position [125, 37, 185, 68] ZOrder 19 - FunctionName "WBToolbox" - Parameters "'CentroidalMomentum','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName" + FunctionName "BlockFactory" + Parameters "'ForwardKinematics','WBToolbox',WBTConfigParameters,configBlockAbsName,frameName" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off } Block { BlockType Outport - Name "Centroidal Momentum" - SID "1700" - Position [315, 73, 345, 87] + Name "ForwardKinematics" + SID "1651" + Position [245, 48, 275, 62] ZOrder 25 IconDisplay "Port number" } @@ -3943,13 +3135,14 @@ Library { ZOrder 1 SrcBlock "S-Function" SrcPort 1 - DstBlock "Centroidal Momentum" + DstBlock "ForwardKinematics" DstPort 1 } Line { ZOrder 2 SrcBlock "Base Pose" SrcPort 1 + Points [36, 0; 0, 15] DstBlock "S-Function" DstPort 1 } @@ -3957,86 +3150,202 @@ Library { ZOrder 3 SrcBlock "Joint configuration" SrcPort 1 + Points [33, 0; 0, -10] 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, 212, 540, 313] - ZOrder 73 + Name "InverseKinematics" + 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 Object { $PropName "MaskObject" - $ObjectID 186 + $ObjectID 118 $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 intern" - "al degrees 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 configur" - "ation of the joints.\n - Base velocity: Vector of size 6 representing the linear and angular velocity of the base f" - "rame.\n - Joints velocity: Vector of size DoFs, representing the velocity of the joints.\n\nOutput:\n - Bias Force" - "s: a Dofs + 6 vector 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 "off" - Object { - $PropName "DialogControls" - $ObjectID 187 - $ClassName "Simulink.dialog.Group" - Prompt "%" - Array { - Type "Simulink.dialog.Control" - Dimension 2 - Object { - $ObjectID 188 + Type "ForwardKinematics" + Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " + "internal degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformatio" + "n between\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, represe" + "nting the configuration \n of the joints.\n\nOutput:\n- ForwardKinematics: a 4x4 matrix repre" + "senting the 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 por" + "ts opened 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 un" + "derlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Inte" + "rface\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" + RunInitForIconRedraw "on" + Array { + Type "Simulink.MaskParameter" + Dimension 7 + Object { + $ObjectID 119 + Type "edit" + Name "baseFrame" + Prompt "Base Frame" + Value "'root_link'" + } + Object { + $ObjectID 120 + Type "edit" + Name "endEffFrame" + Prompt "End Effector frame" + Value "'l_sole'" + } + Object { + $ObjectID 121 + 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 122 + Type "edit" + Name "robotName" + Prompt "Robot Port Name" + Value "WBT_robotName" + Tunable "off" + } + Object { + $ObjectID 123 + Type "edit" + Name "localName" + Prompt "Model Name" + Value "WBT_modelName" + Tunable "off" + } + Object { + $ObjectID 124 + Type "edit" + Name "wbiFile" + Prompt "WBI filename" + Value "WBT_wbiFilename" + Tunable "off" + } + Object { + $ObjectID 125 + Type "edit" + Name "wbiList" + Prompt "WBI list name" + Value "WBT_wbiList" + Tunable "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 126 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 127 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } - Object { - $ObjectID 189 - $ClassName "Simulink.dialog.Group" + Name "DescGroupVar" + } + Object { + $ObjectID 128 + Prompt "Simulink:studio:ToolBarParametersMenu" Object { $PropName "DialogControls" - $ObjectID 190 - $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" + $ObjectID 129 + $ClassName "Simulink.dialog.TabContainer" + Array { + Type "Simulink.dialog.Tab" + Dimension 2 + Object { + $ObjectID 130 + Prompt "Block parameters" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 131 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "baseFrame" } - Name "ToggleButtonContainer" + Object { + $ObjectID 132 + $ClassName "Simulink.dialog.parameter.Edit" + Name "endEffFrame" + } + Object { + $ObjectID 133 + $ClassName "Simulink.dialog.parameter.Popup" + Name "optOption" + } + PropName "DialogControls" + } + Name "Container8" + } + Object { + $ObjectID 134 + Prompt "WBI parameters" + Array { + Type "Simulink.dialog.parameter.Edit" + Dimension 4 + Object { + $ObjectID 135 + Name "robotName" + } + Object { + $ObjectID 136 + Name "localName" + } + Object { + $ObjectID 137 + Name "wbiFile" + } + Object { + $ObjectID 138 + Name "wbiList" + } + PropName "DialogControls" + } + Name "Container5" } PropName "DialogControls" + } + Name "Container4" + } + Name "ParameterGroupVar" } - Name "DescGroupVar" + PropName "DialogControls" } } System { - Name "Get Bias Forces" - Location [832, 225, 3700, 1914] + Name "InverseKinematics" + Location [0, 23, 1280, 744] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -4051,186 +3360,180 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "715" + ZoomFactor "312" Block { BlockType Inport - Name "Base Pose" - SID "1776" - Position [10, 38, 40, 52] - ZOrder 22 + Name "Desired frame pose" + SID "1759" + Position [10, 13, 40, 27] + ZOrder 26 IconDisplay "Port number" } Block { BlockType Inport - Name "Joint configuration" - SID "1777" - Position [10, 68, 40, 82] - ZOrder 24 + Name "Current Base Pose" + SID "1755" + Position [10, 43, 40, 57] + ZOrder 22 Port "2" IconDisplay "Port number" } Block { BlockType Inport - Name "Base velocity" - SID "1778" - Position [10, 98, 40, 112] - ZOrder 26 + Name "Current Joint configuration" + SID "1756" + Position [10, 73, 40, 87] + ZOrder 24 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] + SID "1757" + Ports [3, 2] + Position [145, 4, 225, 96] ZOrder 19 FunctionName "WBToolbox" - Parameters "'InverseDynamics','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName" + Parameters "'InverseKinematics',robotName,localName,wbiFile,wbiList,baseFrame, endEffFrame,optOption" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off } Block { BlockType Outport - Name "Bias Forces" - SID "1783" - Position [300, 113, 330, 127] + 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 "Bias Forces" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Base Pose" - SrcPort 1 - DstBlock "S-Function" + DstBlock "Desired Base Pose" DstPort 1 } Line { - ZOrder 3 - SrcBlock "Joint configuration" + ZOrder 5 + SrcBlock "Current Base Pose" SrcPort 1 DstBlock "S-Function" DstPort 2 } Line { ZOrder 4 - SrcBlock "Base velocity" + SrcBlock "Current Joint configuration" 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" + ZOrder 7 + SrcBlock "Desired frame pose" SrcPort 1 DstBlock "S-Function" - DstPort 5 + DstPort 1 } Line { - ZOrder 9 - SrcBlock "Gain" - SrcPort 1 - DstBlock "S-Function" - DstPort 6 + ZOrder 8 + SrcBlock "S-Function" + SrcPort 2 + DstBlock "Desired Joint Configuration" + DstPort 1 } } } Block { BlockType SubSystem - Name "Get Gravity Forces" - SID "1733" - Ports [2, 1] - Position [600, 210, 740, 310] - ZOrder 74 + Name "RelativeTransform" + SID "1799" + Ports [1, 1] + Position [565, 198, 715, 262] + ZOrder 67 RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 191 + $ObjectID 139 $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 in" - "ternal degrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformat" - "ion between the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the conf" - "iguration of the joints.\n\nOutput:\n - Gravity: a DoFs + 6 vector representing the torques due to the gravity." + Type "RelativeTransform" + Description "This block calculates the relative transform between two frames.\n\nAssuming DoFs is the number of int" + "ernal degrees of freedom of the robot:\n\nInput:\n - Joint configuration: Vector of size DoFs, representing the con" + "figuration of the joints.\n\nOutput:\n - Transform: a 4x4 matrix representing the homogenous transformation between" + " frame1 and frame2.\n\nParameters:\n - Frame1 name: the name of the first frame. It should be part of the URDF mode" + "l.\n - Frame2 name: the name of the second frame. It should be part of 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" - RunInitForIconRedraw "off" - Object { - $PropName "DialogControls" - $ObjectID 192 - $ClassName "Simulink.dialog.Group" - Prompt "%" - Array { - Type "Simulink.dialog.Control" - Dimension 2 + Display "port_label('input', 1, 'Joint configuration')\nport_label('output', 1, strcat('{}^{', frame1, '}', 'H_{'," + " frame2, '}'), 'texmode','on')" + RunInitForIconRedraw "on" + Array { + Type "Simulink.MaskParameter" + Dimension 2 + Object { + $ObjectID 140 + Type "edit" + Name "frame1" + Prompt "Frame1 name" + Value "'frame1'" + } + Object { + $ObjectID 141 + Type "edit" + Name "frame2" + Prompt "Frame2 name" + Value "'frame2'" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 142 + Prompt "%" Object { - $ObjectID 193 + $PropName "DialogControls" + $ObjectID 143 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } + Name "DescGroupVar" + } + Object { + $ObjectID 144 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 3 + Object { + $ObjectID 145 + $ClassName "Simulink.dialog.parameter.Edit" + Name "frame1" + } + Object { + $ObjectID 146 + $ClassName "Simulink.dialog.parameter.Edit" + Name "frame2" + } Object { - $ObjectID 194 + $ObjectID 147 $ClassName "Simulink.dialog.Group" Object { $PropName "DialogControls" - $ObjectID 195 + $ObjectID 148 $ClassName "Simulink.dialog.Button" Prompt "Toggle Config Block Highlighting" Callback "[configBlock, ~] = WBToolbox.BlockInitialization(gcb, gcs);\n\ntry\n if (strcmp(get_param(conf" @@ -4242,13 +3545,15 @@ Library { Name "ToggleButtonContainer" } PropName "DialogControls" + } + Name "ParameterGroupVar" } - Name "DescGroupVar" + PropName "DialogControls" } } System { - Name "Get Gravity Forces" - Location [832, 225, 3700, 1914] + Name "RelativeTransform" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -4263,260 +3568,184 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "715" - Block { - BlockType Inport - Name "Base Pose" - SID "1784" - Position [10, 38, 40, 52] - ZOrder 22 - IconDisplay "Port number" - } + ZoomFactor "834" Block { BlockType Inport Name "Joint configuration" - SID "1785" - Position [10, 68, 40, 82] - ZOrder 24 - Port "2" + SID "1800" + Position [45, 48, 75, 62] + ZOrder 22 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','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName" + SID "1806" + Ports [1, 1] + Position [120, 40, 180, 70] + ZOrder 27 + FunctionName "BlockFactory" + Parameters "'RelativeTransform','WBToolbox',WBTConfigParameters,configBlockAbsName,frame1, frame2" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off } Block { BlockType Outport - Name "Gravity Torques" - SID "1789" - Position [300, 113, 330, 127] + Name "RelativeTransform" + SID "1803" + Position [225, 48, 255, 62] ZOrder 25 IconDisplay "Port number" } Line { - ZOrder 1 + ZOrder 4 SrcBlock "S-Function" SrcPort 1 - DstBlock "Gravity Torques" - DstPort 1 - } - Line { - ZOrder 2 - SrcBlock "Base Pose" - SrcPort 1 - DstBlock "S-Function" + DstBlock "RelativeTransform" 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 - } + DstBlock "S-Function" + DstPort 1 } } } Block { - BlockType S-Function - Name "Inverse Dynamics" - SID "1748" - Ports [6, 1] - Position [190, 199, 355, 331] - ZOrder 75 - FunctionName "WBToolbox" - Parameters "'InverseDynamics','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off + BlockType SubSystem + Name "RemoteInverseKinematics" + 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 Object { $PropName "MaskObject" - $ObjectID 196 + $ObjectID 149 $ClassName "Simulink.Mask" - Type "Inverse Dynamics" - Description "This block compute the inverse dynamics of the robot.\n\nAssuming DoFs is the number of internal degre" - "es 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 - 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 corre" - "sponding torques required 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 "off" - Object { - $PropName "DialogControls" - $ObjectID 197 - $ClassName "Simulink.dialog.Group" - Prompt "%" - Array { - Type "Simulink.dialog.Control" - Dimension 2 - Object { - $ObjectID 198 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Object { - $ObjectID 199 - $ClassName "Simulink.dialog.Group" - Object { - $PropName "DialogControls" - $ObjectID 200 - $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" + Type "ForwardKinematics" + Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " + "internal degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformatio" + "n between\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, represe" + "nting the configuration \n of the joints.\n\nOutput:\n- ForwardKinematics: a 4x4 matrix repre" + "senting the 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 por" + "ts opened 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 un" + "derlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Inte" + "rface\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" + RunInitForIconRedraw "on" + Array { + Type "Simulink.MaskParameter" + Dimension 3 + Object { + $ObjectID 150 + Type "edit" + Name "solverName" + Prompt "Solver Name" + Value "'/cartesianSolver'" + } + Object { + $ObjectID 151 + Type "edit" + Name "dofs" + Prompt "#Dofs" + Value "12" + Tunable "off" + } + Object { + $ObjectID 152 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "Full Constraint (Position and Orientation)" + Cell "Position only constraint" + PropName "TypeOptions" } - PropName "DialogControls" + Name "optOption" + Prompt "Optimization Option" + Value "Full Constraint (Position and Orientation)" } - Name "DescGroupVar" + PropName "Parameters" } - } - } - Block { - BlockType SubSystem - Name "Mass Matrix" - SID "1633" - Ports [2, 1] - Position [250, 104, 390, 171] - ZOrder 32 - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 201 - $ClassName "Simulink.Mask" - Type "Mass Matrix" - Description "This block retrieves the robot mass matrix.\n\nAssuming DoFs is the number of internal degrees of free" - "dom of the robot,\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformation between the base f" - "rame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the configuration of the joint" - "s.\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 "off" - Object { - $PropName "DialogControls" - $ObjectID 202 - $ClassName "Simulink.dialog.Group" - Prompt "%" - Array { - Type "Simulink.dialog.Control" - Dimension 2 + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 153 + Prompt "%" Object { - $ObjectID 203 + $PropName "DialogControls" + $ObjectID 154 $ClassName "Simulink.dialog.Text" Prompt "%" Name "DescTextVar" } + Name "DescGroupVar" + } + Object { + $ObjectID 155 + Prompt "Simulink:studio:ToolBarParametersMenu" Object { - $ObjectID 204 - $ClassName "Simulink.dialog.Group" + $PropName "DialogControls" + $ObjectID 156 + $ClassName "Simulink.dialog.TabContainer" Object { $PropName "DialogControls" - $ObjectID 205 - $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" + $ObjectID 157 + $ClassName "Simulink.dialog.Tab" + Prompt "Block parameters" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 158 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "solverName" } - Name "ToggleButtonContainer" + Object { + $ObjectID 159 + $ClassName "Simulink.dialog.parameter.Edit" + Name "dofs" + } + Object { + $ObjectID 160 + $ClassName "Simulink.dialog.parameter.Popup" + Name "optOption" } PropName "DialogControls" + } + Name "Container8" + } + Name "Container4" + } + Name "ParameterGroupVar" } - Name "DescGroupVar" + PropName "DialogControls" } } System { - Name "Mass Matrix" - Location [832, 225, 3700, 1914] + Name "RemoteInverseKinematics" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -4531,20 +3760,20 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "952" + ZoomFactor "469" Block { BlockType Inport - Name "Base Pose" - SID "1634" - Position [20, 23, 50, 37] - ZOrder 22 + Name "Desired frame pose" + SID "1762" + Position [10, 23, 40, 37] + ZOrder 26 IconDisplay "Port number" } Block { BlockType Inport - Name "Joint configuration" - SID "1635" - Position [20, 63, 50, 77] + Name "Current Joint configuration" + SID "1763" + Position [10, 68, 40, 82] ZOrder 24 Port "2" IconDisplay "Port number" @@ -4552,44 +3781,42 @@ Library { Block { BlockType S-Function Name "S-Function" - SID "1636" + SID "1764" Ports [2, 1] - Position [125, 37, 185, 68] + Position [145, 6, 225, 99] ZOrder 19 FunctionName "WBToolbox" - Parameters "'MassMatrix','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName" + Parameters "'RemoteInverseKinematics',solverName, dofs, optOption" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off } Block { BlockType Outport - Name "Mass Matrix" - SID "1637" - Position [245, 48, 275, 62] - ZOrder 25 + 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 "Mass Matrix" + DstBlock "Desired Joint Configuration" DstPort 1 } Line { ZOrder 2 - SrcBlock "Base Pose" + SrcBlock "Desired frame pose" SrcPort 1 - Points [36, 0; 0, 15] DstBlock "S-Function" DstPort 1 } Line { ZOrder 3 - SrcBlock "Joint configuration" + SrcBlock "Current Joint configuration" SrcPort 1 - Points [33, 0; 0, -10] DstBlock "S-Function" DstPort 2 } @@ -4597,422 +3824,173 @@ Library { } } } + } + } + Block { + BlockType SubSystem + Name "States" + SID "206" + Ports [] + Position [16, 17, 114, 114] + ZOrder -4 + ForegroundColor "white" + DropShadow on + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 161 + $ClassName "Simulink.Mask" + Display "image(imread('wholeBodyStates.png'),'center');" + } + System { + Name "States" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "696" Block { BlockType SubSystem - Name "Jacobians" - SID "202" - Ports [] - Position [217, 20, 324, 127] - ZOrder -3 - DropShadow on + Name "GetLimits" + SID "1690" + Ports [0, 2] + Position [345, 225, 460, 260] + ZOrder 68 + BackgroundColor "[0.513700, 0.851000, 0.670600]" RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 206 + $ObjectID 162 $ClassName "Simulink.Mask" - Display "image(imread('jacobian.png'))" - } - System { - Name "Jacobians" - Location [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "728" - Block { - BlockType SubSystem - Name "DotJ Nu" - SID "1683" - Ports [4, 1] - Position [590, 170, 755, 265] - ZOrder 67 - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 207 - $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 internal degrees of freedom of the r" - "obot:\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 - " - "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 6D vector representing th" - "e product between the time derivative of the Jacobian of the specified frame and the state velocity vector\n\nParame" - "ters:\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" + Type "GetLimits" + Description "This block provides the joint limits gathering data from either the Robot's Control Board or UR" + "DF model.\n\nOutput:\n - Max: 1xDoFs vector containing the maximum limit\n - Min: 1xDoFs vector containing the" + " maximum limit\n\nUnit of measurement:\n - Position: rad\n - Velocity: rad/sec" + 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 163 + Type "popup" + Array { + Type "Cell" + Dimension 2 + Cell "ControlBoard" + Cell "URDF" + PropName "TypeOptions" + } + Name "limitsSource" + Prompt "Limits Source" + Value "ControlBoard" + Evaluate "off" + 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 164 + 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 165 + Prompt "%" Object { - $PropName "Parameters" - $ObjectID 208 - $ClassName "Simulink.MaskParameter" - Type "edit" - Name "frameName" - Prompt "Frame name" - Value "'frame'" + $PropName "DialogControls" + $ObjectID 166 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } + Name "DescGroupVar" + } + Object { + $ObjectID 167 + Prompt "Simulink:studio:ToolBarParametersMenu" Array { - Type "Simulink.dialog.Group" + Type "Simulink.dialog.parameter.Popup" Dimension 2 Object { - $ObjectID 209 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 210 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" + $ObjectID 168 + Name "limitsSource" } Object { - $ObjectID 211 - Prompt "Simulink:studio:ToolBarParametersMenu" - Array { - Type "Simulink.dialog.Control" - Dimension 2 - Object { - $ObjectID 212 - $ClassName "Simulink.dialog.parameter.Edit" - Name "frameName" - } - Object { - $ObjectID 213 - $ClassName "Simulink.dialog.Group" - Object { - $PropName "DialogControls" - $ObjectID 214 - $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" + $ObjectID 169 + Name "limitsType" } PropName "DialogControls" } + Name "ParameterGroupVar" } - System { - Name "DotJ Nu" - Location [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "964" - 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','WBToolboxLibrary',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 + Object { + $ObjectID 170 + Object { + $PropName "DialogControls" + $ObjectID 171 + $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" } - Block { - BlockType SubSystem - Name "Jacobian" - SID "1663" - Ports [2, 1] - Position [380, 190, 540, 245] - ZOrder 39 - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 215 - $ClassName "Simulink.Mask" - Type "Jacobian" - Description "This block retrieves the Jacobian of the specified frame.\n\nAssuming DoFs is the number of internal d" - "egrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transformation betw" - "een the the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing the configur" - "ation of the joints.\n\nOutput:\n - Jacobian: a 6x6+dofs matrix representing the Jacobian of the specified frame wr" - "itten in 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} 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 216 - $ClassName "Simulink.MaskParameter" - Type "edit" - Name "frameName" - Prompt "Frame name" - Value "'frame'" - } - Array { - Type "Simulink.dialog.Group" - Dimension 2 - Object { - $ObjectID 217 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 218 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 219 - Prompt "Simulink:studio:ToolBarParametersMenu" - Array { - Type "Simulink.dialog.Control" - Dimension 2 - Object { - $ObjectID 220 - $ClassName "Simulink.dialog.parameter.Edit" - Name "frameName" - } - Object { - $ObjectID 221 - $ClassName "Simulink.dialog.Group" - Object { - $PropName "DialogControls" - $ObjectID 222 - $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 [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "899" - 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','WBToolboxLibrary',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 - Object { - $PropName "MaskObject" - $ObjectID 223 - $ClassName "Simulink.Mask" - Display "image(imread('forwardKinematics.png'))" } System { - Name "Kinematics" - Location [832, 225, 3700, 1914] + Name "GetLimits" + Location [0, 27, 1920, 1080] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -5027,762 +4005,1665 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "600" + ZoomFactor "966" + SIDHighWatermark "1773" Block { - BlockType SubSystem - Name "Forward Kinematics" - SID "1647" - Ports [2, 1] - Position [360, 103, 510, 167] - ZOrder 34 - RequestExecContextInheritance off + BlockType S-Function + Name "S-Function" + SID "1690:1691" + Ports [0, 2] + Position [115, 33, 175, 102] + ZOrder 19 + FunctionName "BlockFactory" + Parameters "'GetLimits','WBToolbox',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 "GetMeasurement" + SID "1671" + Ports [0, 1] + Position [290, 158, 385, 192] + ZOrder 53 + BackgroundColor "[0.513700, 0.851000, 0.670600]" + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 172 + $ClassName "Simulink.Mask" + Type "GetMeasurement" + 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 measure" + "ment.\n\nParameters:\n - Control Mode: The control mode. Choose one of the supplied mode.\n\nUnit of measurement" + ":\n - Position: rad\n - Velocity: rad/sec\n - Acceleration: rad/sec/sec\n - Torque: Nm\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 173 + $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 { - $PropName "MaskObject" - $ObjectID 224 - $ClassName "Simulink.Mask" - Type "Forward Kinematics" - Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " - "internal degrees of freedom of the robot:\n\nInput:\n - Base pose: 4x4 matrix representing the homogenous transform" - "ation between the the base frame and the world frame.\n - Joint configuration: Vector of size DoFs, representing th" - "e configuration of the joints.\n\nOutput:\n - Forward Kinematics: a 4x4 matrix representing the homogenous transfor" - "mation between the specified frame and the world frame.\n\nParameters:\n - Frame name: the name of the frame. It sh" - "ould 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, '_', '\\_');\n% port_label('output', 1, strcat('{}^{world} H_{',esca" - "pedFrameName,'}'), 'texmode','on')\n\nport_label('input', 1, '{}^{world} H_{base}', 'texmode','on')\nport_label('inp" - "ut', 2, 'Joint configuration')\n\nclear escapedFrameName;" - RunInitForIconRedraw "on" + $ObjectID 174 + Prompt "%" Object { - $PropName "Parameters" - $ObjectID 225 - $ClassName "Simulink.MaskParameter" - Type "edit" - Name "frameName" - Prompt "Frame name" - Value "'frame'" - } - Array { - Type "Simulink.dialog.Group" - Dimension 2 - Object { - $ObjectID 226 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 227 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 228 - Prompt "Simulink:studio:ToolBarParametersMenu" - Array { - Type "Simulink.dialog.Control" - Dimension 2 - Object { - $ObjectID 229 - $ClassName "Simulink.dialog.parameter.Edit" - Name "frameName" - } - Object { - $ObjectID 230 - $ClassName "Simulink.dialog.Group" - Object { - $PropName "DialogControls" - $ObjectID 231 - $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" + $PropName "DialogControls" + $ObjectID 175 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } + Name "DescGroupVar" } - System { - Name "Forward Kinematics" - Location [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "709" - 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','WBToolboxLibrary',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 + Object { + $ObjectID 176 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 177 + $ClassName "Simulink.dialog.parameter.Popup" + Name "measuredType" } - Line { - ZOrder 3 - SrcBlock "Joint configuration" - SrcPort 1 - Points [33, 0; 0, -10] - DstBlock "S-Function" - DstPort 2 + Name "ParameterGroupVar" + } + Object { + $ObjectID 178 + Object { + $PropName "DialogControls" + $ObjectID 179 + $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 "GetMeasurement" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "1000" 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 - Object { - $PropName "MaskObject" - $ObjectID 232 - $ClassName "Simulink.Mask" - Type "Forward Kinematics" - Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " - "internal degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformatio" - "n between\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, represe" - "nting the configuration \n of the joints.\n\nOutput:\n- Forward Kinematics: a 4x4 matrix repre" - "senting the 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 por" - "ts opened 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 un" - "derlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Inte" - "rface\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" - RunInitForIconRedraw "on" - Array { - Type "Simulink.MaskParameter" - Dimension 7 - Object { - $ObjectID 233 - Type "edit" - Name "baseFrame" - Prompt "Base Frame" - Value "'root_link'" - } - Object { - $ObjectID 234 - Type "edit" - Name "endEffFrame" - Prompt "End Effector frame" - Value "'l_sole'" - } - Object { - $ObjectID 235 - 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 236 - Type "edit" - Name "robotName" - Prompt "Robot Port Name" - Value "WBT_robotName" - Tunable "off" - } - Object { - $ObjectID 237 - Type "edit" - Name "localName" - Prompt "Model Name" - Value "WBT_modelName" - Tunable "off" - } - Object { - $ObjectID 238 - Type "edit" - Name "wbiFile" - Prompt "WBI filename" - Value "WBT_wbiFilename" - Tunable "off" - } - Object { - $ObjectID 239 - Type "edit" - Name "wbiList" - Prompt "WBI list name" - Value "WBT_wbiList" - Tunable "off" - } - PropName "Parameters" - } - Array { - Type "Simulink.dialog.Group" - Dimension 2 - Object { - $ObjectID 240 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 241 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 242 - Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 243 - $ClassName "Simulink.dialog.TabContainer" - Array { - Type "Simulink.dialog.Tab" - Dimension 2 - Object { - $ObjectID 244 - Prompt "Block parameters" - Array { - Type "Simulink.dialog.parameter.Control" - Dimension 3 - Object { - $ObjectID 245 - $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "baseFrame" - } - Object { - $ObjectID 246 - $ClassName "Simulink.dialog.parameter.Edit" - Name "endEffFrame" - } - Object { - $ObjectID 247 - $ClassName "Simulink.dialog.parameter.Popup" - Name "optOption" - } - PropName "DialogControls" - } - Name "Container8" - } - Object { - $ObjectID 248 - Prompt "WBI parameters" - Array { - Type "Simulink.dialog.parameter.Edit" - Dimension 4 - Object { - $ObjectID 249 - Name "robotName" - } - Object { - $ObjectID 250 - Name "localName" - } - Object { - $ObjectID 251 - Name "wbiFile" - } - Object { - $ObjectID 252 - Name "wbiList" - } - PropName "DialogControls" - } - Name "Container5" - } - PropName "DialogControls" - } - Name "Container4" - } - Name "ParameterGroupVar" - } - PropName "DialogControls" - } + BlockType S-Function + Name "S-Function" + SID "1672" + Ports [0, 1] + Position [125, 39, 185, 71] + ZOrder 19 + FunctionName "BlockFactory" + Parameters "'GetMeasurement','WBToolbox',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 + } + } + } + Block { + BlockType SubSystem + Name "GetMotorMeasurement" + SID "1796" + Ports [0, 1] + Position [410, 158, 505, 192] + ZOrder 69 + BackgroundColor "[0.960000, 0.770000, 0.460000]" + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 180 + $ClassName "Simulink.Mask" + Type "GetMeasurement" + 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 measure" + "ment.\n\nParameters:\n - Control Mode: The control mode. Choose one of the supplied mode.\n\nUnit of measurement" + ":\n - PWM: duty cycle expressed as percentage [-100, 100]%\n - Current: values in Ampere\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 181 + $ClassName "Simulink.MaskParameter" + Type "popup" + Array { + Type "Cell" + Dimension 7 + Cell "Motor Position" + Cell "Motor Velocity" + Cell "Motor Acceleration" + Cell "Motor PWM" + Cell "Motor Current" + Cell "Motor Back EMF" + Cell "Motor Torque Constant" + PropName "TypeOptions" } - System { - Name "Inverse Kinematics" - Location [0, 23, 1280, 744] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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" + Name "measuredType" + Prompt "Estimate Type" + Value "Motor Current" + Evaluate "off" + Callback "maskStr = get_param(gcb,'measuredType');\n\nswitch maskStr\n case 'Motor Position'\n " + " set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]');\n case 'Motor Velocity'\n set_par" + "am(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n case 'Motor Acceleration'\n set_param(g" + "cb,'BackgroundColor', '[0.9255, 0.8706, 0.1333, 1.0]');\n case 'Motor PWM'\n set_param(gcb,'Backgroun" + "dColor', '[1, 1, 1, 1.0]');\n case 'Motor Current'\n set_param(gcb,'BackgroundColor', '[0.96, 0.77, 0" + ".46, 1.0]');\n case 'Motor Back EMF'\n set_param(gcb,'BackgroundColor', '[0.8, 0.4, 1.0, 1.0]');\n " + " case 'Motor Torque Constant'\n set_param(gcb,'BackgroundColor', '[0.8, 1.0, 0.4, 1.0]');\n otherwise" + "\n error('Control Type not recognized.')\nend\n\nclear maskStr" + } + Array { + Type "Simulink.dialog.Group" + Dimension 3 + Object { + $ObjectID 182 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 183 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } - Block { - BlockType Inport - Name "Current Joint configuration" - SID "1756" - Position [10, 73, 40, 87] - ZOrder 24 - Port "3" - IconDisplay "Port number" + Name "DescGroupVar" + } + Object { + $ObjectID 184 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 185 + $ClassName "Simulink.dialog.parameter.Popup" + Name "measuredType" } - 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 + Name "ParameterGroupVar" + } + Object { + $ObjectID 186 + Object { + $PropName "DialogControls" + $ObjectID 187 + $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" } - Block { - BlockType Outport - Name "Desired Base Pose" - SID "1758" - Position [280, 23, 310, 37] - ZOrder 25 + Name "Container8" + } + PropName "DialogControls" + } + } + System { + Name "GetMotorMeasurement" + Location [0, 27, 1920, 1080] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "1000" + Block { + BlockType S-Function + Name "S-Function" + SID "1797" + Ports [0, 1] + Position [125, 39, 185, 71] + ZOrder 19 + FunctionName "BlockFactory" + Parameters "'GetMeasurement','WBToolbox',WBTConfigParameters,configBlockAbsName,measuredType" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + } + Block { + BlockType Outport + Name "Estimate" + SID "1798" + Position [210, 48, 240, 62] + ZOrder 25 + IconDisplay "Port number" + } + Line { + ZOrder 1 + SrcBlock "S-Function" + SrcPort 1 + DstBlock "Estimate" + DstPort 1 + } + } + } + } + } + Block { + BlockType SubSystem + Name "Utilities" + SID "192" + Ports [] + Position [364, 17, 462, 114] + ZOrder -1 + ForegroundColor "white" + DropShadow on + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 188 + $ClassName "Simulink.Mask" + Display "image(imread('utilities.png'))" + } + System { + Name "Utilities" + Location [0, 27, 1920, 1080] + Open on + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "219" + Block { + BlockType SubSystem + Name "Configuration" + SID "1774" + Ports [] + Position [295, 310, 370, 340] + ZOrder 95 + InitFcn "source = get_param(gcb,'ConfigSource');\n\nif strcmp(source, 'Workspace')\n object = get_param(gcb,'" + "ConfigObject');\n WBToolbox.ConfigurationToMask(gcb, object);\nend\n\nWBToolbox.MaskToConfiguration(gcb);\n\ncle" + "ar object source" + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 189 + $ClassName "Simulink.Mask" + SelfModifiable "on" + Display "disp('Config')" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 8 + Object { + $ObjectID 190 + 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.ConfigurationToMask(gcb,curr" + "entConfigObject);\n clear currentConfigObject;\nelseif strcmp(currentConfigSource,'Mask')\n % Switch the visib" + "ility 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.Pa" + "rameters(6).Enabled = 'on';\n h.Parameters(7).Enabled = 'on';\n h.Parameters(8).Enabled = 'on';\nend\n\nclear " + "h currentConfigSource" + } + Object { + $ObjectID 191 + 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.ConfigurationToMask(gcb,currentConfigObject);\n\nclear currentConfigObject;" + } + Object { + $ObjectID 192 + Type "edit" + Name "RobotName" + Prompt "Robot Name" + Value "'icubSim'" + } + Object { + $ObjectID 193 + Type "edit" + Name "UrdfFile" + Prompt "Urdf File" + Value "'model.urdf'" + } + Object { + $ObjectID 194 + Type "edit" + Name "ControlledJoints" + Prompt "Controlled Joints" + Value "{'l_elbow','l_shoulder_pitch','torso_roll'}" + } + Object { + $ObjectID 195 + Type "edit" + Name "ControlBoardsNames" + Prompt "Control Boards Names" + Value "{'left_arm','right_arm','torso'}" + } + Object { + $ObjectID 196 + Type "edit" + Name "LocalName" + Prompt "Local Name" + Value "'WBT'" + } + Object { + $ObjectID 197 + Type "edit" + Name "GravityVector" + Prompt "Gravity Vector" + Value "[0,0,-9.81]" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 198 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 199 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 200 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 201 + $ClassName "Simulink.dialog.TabContainer" + Array { + Type "Simulink.dialog.Tab" + Dimension 2 + Object { + $ObjectID 202 + Prompt "From" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 2 + Object { + $ObjectID 203 + $ClassName "Simulink.dialog.parameter.Popup" + Name "ConfigSource" + } + Object { + $ObjectID 204 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "ConfigObject" + } + PropName "DialogControls" + } + Name "TabFrom" + } + Object { + $ObjectID 205 + Prompt "Data" + Array { + Type "Simulink.dialog.parameter.Edit" + Dimension 6 + Object { + $ObjectID 206 + PromptLocation "left" + Name "RobotName" + } + Object { + $ObjectID 207 + PromptLocation "left" + Name "UrdfFile" + } + Object { + $ObjectID 208 + PromptLocation "left" + Name "ControlledJoints" + } + Object { + $ObjectID 209 + PromptLocation "left" + Name "ControlBoardsNames" + } + Object { + $ObjectID 210 + PromptLocation "left" + Name "LocalName" + } + Object { + $ObjectID 211 + 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 + SetDomainSpec off + DomainSpecType "Deduce" + 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 [495, 198, 565, 242] + ZOrder -1 + BackgroundColor "[0.848000, 0.128048, 0.320035]" + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 212 + $ClassName "Simulink.Mask" + RunInitForIconRedraw "off" + Object { + $PropName "Parameters" + $ObjectID 213 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "tol" + Prompt "Tolerance" + Value "1e-4" + } + } + System { + Name "DampPinv" + Location [0, 29, 1280, 744] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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" + System { + Name "Damped Pseudo Inverse" + Location [12, 45, 1279, 3773] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "1628" + Block { + BlockType Inport + Name "mat" + SID "107::1" + Position [20, 101, 40, 119] + ZOrder -1 IconDisplay "Port number" } Block { - BlockType Outport - Name "Desired Joint Configuration" - SID "1760" - Position [280, 68, 310, 82] - ZOrder 27 + 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::1627" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 107 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "107::1626" + Tag "Stateflow S-Function WBToolboxLibrary_repository 6" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 106 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "DPinv" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "107::1628" + Position [460, 241, 480, 259] + ZOrder 108 + } + Block { + BlockType Outport + Name "DPinv" + SID "107::5" + Position [460, 101, 480, 119] + ZOrder -6 + IconDisplay "Port number" + } Line { - ZOrder 1 - SrcBlock "S-Function" + ZOrder 76 + SrcBlock "mat" SrcPort 1 - DstBlock "Desired Base Pose" + Points [120, 0] + DstBlock " SFunction " DstPort 1 } Line { - ZOrder 5 - SrcBlock "Current Base Pose" + ZOrder 77 + SrcBlock "sigma" SrcPort 1 - DstBlock "S-Function" + DstBlock " SFunction " DstPort 2 } Line { - ZOrder 4 - SrcBlock "Current Joint configuration" + Name "DPinv" + ZOrder 78 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "DPinv" + DstPort 1 + } + Line { + ZOrder 79 + SrcBlock " Demux " SrcPort 1 - DstBlock "S-Function" - DstPort 3 + DstBlock " Terminator " + DstPort 1 } Line { - ZOrder 7 - SrcBlock "Desired frame pose" + ZOrder 80 + SrcBlock " SFunction " SrcPort 1 - DstBlock "S-Function" + DstBlock " Demux " DstPort 1 } - Line { - ZOrder 8 - SrcBlock "S-Function" - SrcPort 2 - DstBlock "Desired Joint Configuration" - 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 [165, 310, 235, 340] + ZOrder 103 + BackgroundColor "yellow" + FunctionName "BlockFactory" + Parameters "'DiscreteFilter','WBToolbox',filterDataStruct" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 214 + $ClassName "Simulink.Mask" + Type "DiscreteFilter" + Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." + Initialization "filterDataStruct = struct;\nfilterDataStruct.Fc = Fc;\nfilterDataStruct.Ts = Ts;\nfilterDataS" + "truct.MedianOrder = orderMedianFilter;\nfilterDataStruct.FilterType = filterType;\nfilterDataStruct.NumCoeffs = n" + "umCoeffs;\nfilterDataStruct.DenCoeffs = denCoeffs;\nfilterDataStruct.y0 = y0;\nfilterDataStruct.u0 = u0;\nfilterD" + "ataStruct.InitStatus = uint8(initStatus);" + Display "disp('Filter')" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 9 + Object { + $ObjectID 215 + Type "popup" + Array { + Type "Cell" + Dimension 3 + Cell "Generic" + Cell "FirstOrderLowPassFilter" + Cell "MedianFilter" + PropName "TypeOptions" + } + Name "filterType" + Prompt "Type of the filter" + Value "Generic" + Evaluate "off" + 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 216 + Type "edit" + Name "numCoeffs" + Prompt "Numerator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 217 + Type "edit" + Name "denCoeffs" + Prompt "Denominator Coefficients*" + Value "[0]" + } + Object { + $ObjectID 218 + Type "edit" + Name "Fc" + Prompt "Cut Frequency (Hz)" + Value "0" + Visible "off" + } + Object { + $ObjectID 219 + Type "edit" + Name "Ts" + Prompt "Sampling time (s)" + Value "0" + Visible "off" + } + Object { + $ObjectID 220 + Type "edit" + Name "orderMedianFilter" + Prompt "Order" + Value "0" + Visible "off" + } + Object { + $ObjectID 221 + 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 222 + Type "edit" + Name "y0" + Prompt "Output y0" + Value "[0]" + Visible "off" + } + Object { + $ObjectID 223 + Type "edit" + Name "u0" + Prompt "Input u0" + Value "[0]" + Visible "off" + } + PropName "Parameters" + } + Array { + Type "Simulink.dialog.Group" + Dimension 2 + Object { + $ObjectID 224 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 225 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 226 + Prompt "Simulink:studio:ToolBarParametersMenu" + Array { + Type "Simulink.dialog.Control" + Dimension 8 + Object { + $ObjectID 227 + $ClassName "Simulink.dialog.parameter.Popup" + Name "filterType" + } + Object { + $ObjectID 228 + $ClassName "Simulink.dialog.parameter.Edit" + Name "numCoeffs" + } + Object { + $ObjectID 229 + $ClassName "Simulink.dialog.parameter.Edit" + Name "denCoeffs" + } + Object { + $ObjectID 230 + $ClassName "Simulink.dialog.Text" + Prompt "* The coefficients are ordered in increasing power of z^-1" + Name "howToCoeffs" + } + Object { + $ObjectID 231 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Fc" + } + Object { + $ObjectID 232 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "Ts" + } + Object { + $ObjectID 233 + $ClassName "Simulink.dialog.parameter.Edit" + PromptLocation "left" + Name "orderMedianFilter" + } + Object { + $ObjectID 234 + $ClassName "Simulink.dialog.Group" + Array { + Type "Simulink.dialog.parameter.Control" + Dimension 3 + Object { + $ObjectID 235 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "initStatus" + } + Object { + $ObjectID 236 + $ClassName "Simulink.dialog.parameter.Edit" + Name "y0" + } + Object { + $ObjectID 237 + $ClassName "Simulink.dialog.parameter.Edit" + Name "u0" + } + PropName "DialogControls" + } + Name "Container3" + } + PropName "DialogControls" } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } + Block { + BlockType SubSystem + Name "MatchSignalSizes" + SID "1818" + Ports [2, 1] + Position [440, 297, 525, 353] + ZOrder 251 + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 238 + $ClassName "Simulink.Mask" + Type "MatchSignalSizes" + Description "It can happen on particular cases that the Simulink engine cannot propagate the right sizes fro" + "m blocks of the toolbox. If you encounter such problems you can try using this blocks which introducing a minimal" + " overhead assigns to input Signal the same size of input Reference and outputs Signal.\n\nCurrently it supports o" + "nly 1D vectors." + Display "port_label('input', 1, 'Reference')\nport_label('input', 2, 'Signal')\nport_label('output', 1, 'Sig" + "nal')" + RunInitForIconRedraw "off" + } + System { + Name "MatchSignalSizes" + Location [548, 183, 3416, 1872] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "1000" + Block { + BlockType Inport + Name "Reference" + SID "1819" + Position [315, 178, 345, 192] + ZOrder 240 + NamePlacement "alternate" + IconDisplay "Port number" + } + Block { + BlockType Inport + Name "Signal" + SID "1820" + Position [315, 198, 345, 212] + ZOrder 241 + Port "2" + IconDisplay "Port number" + } + Block { + BlockType DotProduct + Name "Dot Product" + SID "1836" + Position [410, 177, 445, 213] + ZOrder 252 + OutDataTypeStr "Inherit: Inherit via internal rule" + } + Block { + BlockType Terminator + Name "Terminator" + SID "1822" + Position [485, 185, 505, 205] + ZOrder 245 + } + Block { + BlockType Outport + Name "Output" + SID "1823" + Position [410, 243, 440, 257] + ZOrder 244 + IconDisplay "Port number" + } + Line { + ZOrder 27 + SrcBlock "Dot Product" + SrcPort 1 + DstBlock "Terminator" + DstPort 1 + } + Line { + ZOrder 2 + SrcBlock "Signal" + SrcPort 1 + Points [17, 0] + Branch { + ZOrder 28 + Points [0, 45] + DstBlock "Output" + DstPort 1 + } + Branch { + ZOrder 26 + DstBlock "Dot Product" + DstPort 2 + } + } + Line { + ZOrder 24 + SrcBlock "Reference" + SrcPort 1 + DstBlock "Dot Product" + DstPort 1 + } + } + } + Block { + BlockType S-Function + Name "MinimumJerkTrajectoryGenerator" + SID "1747" + Ports [1, 3] + Position [470, 93, 625, 147] + ZOrder 78 + FunctionName "BlockFactory" + Parameters "'MinimumJerkTrajectoryGenerator','WBToolbox',sampleTime,settlingTime,firstDerivatives,secondD" + "erivatives,explicitInitialValue,externalSettlingTime,resetOnSettlingTime" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 239 + $ClassName "Simulink.Mask" + Type "MinimumJerkTrajectoryGenerator" + Description "This block wraps the minJerkTrajGen class from iCub::ctrl::minJerkTrajGen.\n\nThe Minimum Jerk " + "Trajectory Generator is approximated using a 3rd order LTI dynamical system (for more details see [1]).\n\nPositi" + "on, velocity and acceleration trajectories are computed.\n\nThe main advantage with respect to the standard polyn" + "omial form is that if the reference value yd changes there is no need to recompute the filter coefficients.\n\n[1" + "]\nPattacini, U.; Nori, F.; Natale, L.; Metta, G.; Sandini, G.\n\"An experimental evaluation of a novel minimum-j" + "erk cartesian controller for humanoid robots\"\nIntelligent Robots and Systems (IROS), 2010\nIEEE/RSJ Internation" + "al Conference on , vol., no., pp.1668-1674, 18-22 Oct. 2010\ndoi: 10.1109/IROS.2010.5650851\nURL: http://ieeexplo" + "re.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" + RunInitForIconRedraw "on" + Array { + Type "Simulink.MaskParameter" + Dimension 7 + Object { + $ObjectID 240 + 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 241 + Type "edit" + Name "settlingTime" + Prompt "Settling Time" + Value "3" + } + Object { + $ObjectID 242 + Type "edit" + Name "sampleTime" + Prompt "Sample Time" + Value "0.01" + } + Object { + $ObjectID 243 + Type "checkbox" + Name "resetOnSettlingTime" + Prompt "Reset on Settling Time Changes" + Value "off" + Visible "off" + } + Object { + $ObjectID 244 + Type "checkbox" + Name "firstDerivatives" + Prompt "Output First Derivative" + Value "on" + } + Object { + $ObjectID 245 + Type "checkbox" + Name "secondDerivatives" + Prompt "Output Second Derivative" + Value "on" } + Object { + $ObjectID 246 + Type "checkbox" + Name "explicitInitialValue" + Prompt "Explicit Initial Value" + Value "off" + Visible "off" + } + PropName "Parameters" } - Block { - BlockType SubSystem - Name "Relative Transform" - SID "1799" - Ports [1, 1] - Position [565, 198, 715, 262] - ZOrder 67 - RequestExecContextInheritance off + Array { + Type "Simulink.dialog.Group" + Dimension 2 Object { - $PropName "MaskObject" - $ObjectID 253 - $ClassName "Simulink.Mask" - Type "Relative Transform" - Description "This block calculates the relative transform between two frames.\n\nAssuming DoFs is the number of int" - "ernal degrees of freedom of the robot:\n\nInput:\n - Joint configuration: Vector of size DoFs, representing the con" - "figuration of the joints.\n\nOutput:\n - Transform: a 4x4 matrix representing the homogenous transformation between" - " frame1 and frame2.\n\nParameters:\n - Frame1 name: the name of the first frame. It should be part of the URDF mode" - "l.\n - Frame2 name: the name of the second frame. It should be part of 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 "port_label('input', 1, 'Joint configuration')\nport_label('output', 1, strcat('{}^{', frame1, '}', 'H_{'," - " frame2, '}'), 'texmode','on')" - RunInitForIconRedraw "on" - Array { - Type "Simulink.MaskParameter" - Dimension 2 - Object { - $ObjectID 254 - Type "edit" - Name "frame1" - Prompt "Frame1 name" - Value "'frame1'" - } - Object { - $ObjectID 255 - Type "edit" - Name "frame2" - Prompt "Frame2 name" - Value "'frame2'" - } - PropName "Parameters" + $ObjectID 247 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 248 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" } - Array { - Type "Simulink.dialog.Group" - Dimension 2 - Object { - $ObjectID 256 - Prompt "%" + Name "DescGroupVar" + } + Object { + $ObjectID 249 + Prompt "Simulink:studio:ToolBarParametersMenu" + Object { + $PropName "DialogControls" + $ObjectID 250 + $ClassName "Simulink.dialog.TabContainer" + Array { + Type "Simulink.dialog.Tab" + Dimension 2 Object { - $PropName "DialogControls" - $ObjectID 257 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 258 - Prompt "Simulink:studio:ToolBarParametersMenu" + $ObjectID 251 + Prompt "Trajectory Parameters" Array { - Type "Simulink.dialog.Control" - Dimension 3 + Type "Simulink.dialog.parameter.Control" + Dimension 4 Object { - $ObjectID 259 + $ObjectID 252 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "externalSettlingTime" + } + Object { + $ObjectID 253 $ClassName "Simulink.dialog.parameter.Edit" - Name "frame1" + Name "settlingTime" } Object { - $ObjectID 260 + $ObjectID 254 $ClassName "Simulink.dialog.parameter.Edit" - Name "frame2" + Name "sampleTime" } Object { - $ObjectID 261 - $ClassName "Simulink.dialog.Group" + $ObjectID 255 + $ClassName "Simulink.dialog.parameter.CheckBox" + Name "resetOnSettlingTime" + } + PropName "DialogControls" + } + Name "Tab1" + } Object { - $PropName "DialogControls" - $ObjectID 262 - $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" + $ObjectID 256 + Prompt "Input/Output" + Array { + Type "Simulink.dialog.parameter.CheckBox" + Dimension 3 + Object { + $ObjectID 257 + Name "firstDerivatives" } - Name "ToggleButtonContainer" + Object { + $ObjectID 258 + Name "secondDerivatives" + } + Object { + $ObjectID 259 + Name "explicitInitialValue" } PropName "DialogControls" } - Name "ParameterGroupVar" + Name "Tab2" + } + PropName "DialogControls" } - PropName "DialogControls" + Name "Container3" } + Name "ParameterGroupVar" } - System { - Name "Relative Transform" - Location [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "834" - Block { - BlockType Inport - Name "Joint configuration" - SID "1800" - Position [45, 48, 75, 62] - ZOrder 22 - IconDisplay "Port number" - } - Block { - BlockType S-Function - Name "S-Function" - SID "1806" - Ports [1, 1] - Position [120, 40, 180, 70] - ZOrder 27 - FunctionName "WBToolbox" - Parameters "'RelativeTransform','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName,frame1, frame2" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off - } - Block { - BlockType Outport - Name "Relative Transform" - SID "1803" - Position [225, 48, 255, 62] - ZOrder 25 - IconDisplay "Port number" - } - Line { - ZOrder 4 - SrcBlock "S-Function" - SrcPort 1 - DstBlock "Relative Transform" - DstPort 1 - } - Line { - ZOrder 3 - SrcBlock "Joint configuration" - SrcPort 1 - DstBlock "S-Function" - DstPort 1 - } + PropName "DialogControls" + } + } + } + Block { + BlockType S-Function + Name "QP" + SID "1807" + Ports [4, 2] + Position [405, -6, 595, 66] + ZOrder 104 + FunctionName "BlockFactory" + Parameters "'QpOases','WBToolbox',lbA,ubA,lb,ub,computeObjVal,stopIfFails" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 260 + $ClassName "Simulink.Mask" + Type "QP" + Description "This block solves a QP problem using the qpOASES library:\n\nx = argmin 0.5 * x' * H * x + c' *" + " x\n\ns.t.\n\nlbA <= Ax <= ubA\nlb <= x <= ub \n" + Display "disp('QP')\n\nport_label('output', 1, 'PrimalSolution')\nport_label('output', 2, 'Status')\n\nif (c" + "omputeObjVal)\n port_label('output', 3, 'Obj func value')\nend\n\nport_label('input', 1, 'H');\nport_label('in" + "put', 2, 'g');\n\ninputPortNumber = 2;\n\nif (lbA || ubA)\n inputPortNumber = 3;\n port_label('input', 3, '" + "A');\nend\n\nif (lbA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lbA'" + ");\nend\nif (ubA)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ubA');\n" + "end\nif (lb)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'lb');\nend\ni" + "f (ub)\n inputPortNumber = inputPortNumber + 1;\n port_label('input', inputPortNumber, 'ub');\nend" + RunInitForIconRedraw "on" + Array { + Type "Simulink.MaskParameter" + Dimension 6 + Object { + $ObjectID 261 + Type "checkbox" + Name "lbA" + Prompt "Accept constraints lower bound input (lbA)" + Value "off" + } + Object { + $ObjectID 262 + Type "checkbox" + Name "ubA" + Prompt "Accept constraints upper bound input (ubA)" + Value "on" + } + Object { + $ObjectID 263 + Type "checkbox" + Name "lb" + Prompt "Accept lower bound input (lb)" + Value "off" + } + Object { + $ObjectID 264 + Type "checkbox" + Name "ub" + Prompt "Accept upper bound input (ub)" + Value "off" + } + Object { + $ObjectID 265 + Type "checkbox" + Name "computeObjVal" + Prompt "Output the value of the objective function" + Value "off" } + Object { + $ObjectID 266 + Type "checkbox" + Name "stopIfFails" + Prompt "Stop the simulation if the solver fails" + Value "off" + } + PropName "Parameters" } - 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 + Array { + Type "Simulink.dialog.Group" + Dimension 2 Object { - $PropName "MaskObject" - $ObjectID 263 - $ClassName "Simulink.Mask" - Type "Forward Kinematics" - Description "This block retrieves the forward kinematics of the specified frame.\n\nAssuming DoFs is the number of " - "internal degrees of freedom of the robot,\nInput:\n- Base pose: 4x4 matrix representing the homogenous transformatio" - "n between\n the the base frame and the world frame.\n- Joint configuration: Vector of size DoFs, represe" - "nting the configuration \n of the joints.\n\nOutput:\n- Forward Kinematics: a 4x4 matrix repre" - "senting the 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 por" - "ts opened 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 un" - "derlying Whole Body Interface.\n- WBI filename: name of the file containing the configuration of the Whole Body Inte" - "rface\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" - RunInitForIconRedraw "on" + $ObjectID 267 + Prompt "%" + Object { + $PropName "DialogControls" + $ObjectID 268 + $ClassName "Simulink.dialog.Text" + Prompt "%" + Name "DescTextVar" + } + Name "DescGroupVar" + } + Object { + $ObjectID 269 + Prompt "Simulink:studio:ToolBarParametersMenu" Array { - Type "Simulink.MaskParameter" + Type "Simulink.dialog.Group" Dimension 3 Object { - $ObjectID 264 - Type "edit" - Name "solverName" - Prompt "Solver Name" - Value "'/cartesianSolver'" - } - Object { - $ObjectID 265 - Type "edit" - Name "dofs" - Prompt "#Dofs" - Value "12" - Tunable "off" - } - Object { - $ObjectID 266 - Type "popup" + $ObjectID 270 + Prompt "Constraints bounds" Array { - Type "Cell" + Type "Simulink.dialog.parameter.CheckBox" Dimension 2 - Cell "Full Constraint (Position and Orientation)" - Cell "Position only constraint" - PropName "TypeOptions" + Object { + $ObjectID 271 + Name "lbA" } - Name "optOption" - Prompt "Optimization Option" - Value "Full Constraint (Position and Orientation)" - } - PropName "Parameters" - } - Array { - Type "Simulink.dialog.Group" - Dimension 2 - Object { - $ObjectID 267 - Prompt "%" Object { - $PropName "DialogControls" - $ObjectID 268 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" + $ObjectID 272 + Name "ubA" } - Name "DescGroupVar" + PropName "DialogControls" + } + Name "constraintsBoundsBox" } Object { - $ObjectID 269 - Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 270 - $ClassName "Simulink.dialog.TabContainer" - Object { - $PropName "DialogControls" - $ObjectID 271 - $ClassName "Simulink.dialog.Tab" - Prompt "Block parameters" + $ObjectID 273 + Prompt "Simple bounds" Array { - Type "Simulink.dialog.parameter.Control" - Dimension 3 - Object { - $ObjectID 272 - $ClassName "Simulink.dialog.parameter.Edit" - PromptLocation "left" - Name "solverName" - } + Type "Simulink.dialog.parameter.CheckBox" + Dimension 2 Object { - $ObjectID 273 - $ClassName "Simulink.dialog.parameter.Edit" - Name "dofs" + $ObjectID 274 + Name "lb" } Object { - $ObjectID 274 - $ClassName "Simulink.dialog.parameter.Popup" - Name "optOption" + $ObjectID 275 + Name "ub" } PropName "DialogControls" } - Name "Container8" + Name "simpleBoundsBox" + } + Object { + $ObjectID 276 + Prompt "Other" + Array { + Type "Simulink.dialog.parameter.CheckBox" + Dimension 2 + Object { + $ObjectID 277 + Name "computeObjVal" } - Name "Container4" + Object { + $ObjectID 278 + Name "stopIfFails" } - Name "ParameterGroupVar" + PropName "DialogControls" + } + Name "otherBox" } PropName "DialogControls" } + Name "ParameterGroupVar" + } + PropName "DialogControls" + } + } + } + Block { + BlockType S-Function + Name "Real Time Synchronizer" + SID "1657" + Ports [] + Position [70, 4, 195, 41] + ZOrder 23 + ForegroundColor "[0.917647, 0.917647, 0.917647]" + BackgroundColor "gray" + ShowName off + FunctionName "BlockFactory" + Parameters "'RealTimeSynchronizer','WBToolbox',period" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 279 + $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')" + RunInitForIconRedraw "off" + Object { + $PropName "Parameters" + $ObjectID 280 + $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 [230, 4, 360, 41] + ZOrder 24 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + ShowName off + FunctionName "BlockFactory" + Parameters "'SimulatorSynchronizer','WBToolbox',period, serverPortName, clientPortName" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 281 + $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')" + RunInitForIconRedraw "off" + Array { + Type "Simulink.MaskParameter" + Dimension 3 + Object { + $ObjectID 282 + Type "edit" + Name "period" + Prompt "Controller Period (in seconds)" + Value "0.01" + } + Object { + $ObjectID 283 + Type "edit" + Name "serverPortName" + Prompt "Server Port Name" + Value "'/clock/rpc'" + } + Object { + $ObjectID 284 + 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 [375, 198, 445, 242] + ZOrder -3 + BackgroundColor "[0.534601, 0.470279, 1.000000]" + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 285 + $ClassName "Simulink.Mask" + RunInitForIconRedraw "off" + Object { + $PropName "Parameters" + $ObjectID 286 + $ClassName "Simulink.MaskParameter" + Type "edit" + Name "tol" + Prompt "Tolerance" + Value "1e-4" + } + } + System { + Name "TruncPinv" + Location [0, 29, 1280, 744] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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" System { - Name "Remote Inverse Kinematics" - Location [853, 51, 2214, 1013] + Name "Truncated PseudoInverse" + Location [12, 45, 1279, 3773] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -5797,394 +5678,341 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "375" + ZoomFactor "100" + SIDHighWatermark "1619" Block { BlockType Inport - Name "Desired frame pose" - SID "1762" - Position [10, 23, 40, 37] - ZOrder 26 + Name "mat" + SID "112::1" + Position [20, 101, 40, 119] + ZOrder -1 IconDisplay "Port number" } Block { BlockType Inport - Name "Current Joint configuration" - SID "1763" - Position [10, 68, 40, 82] - ZOrder 24 + Name "tol" + SID "112::25" + Position [20, 136, 40, 154] + ZOrder 11 Port "2" IconDisplay "Port number" } + Block { + BlockType Demux + Name " Demux " + SID "112::1618" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 107 + Outputs "1" + } 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" + Name " SFunction " + SID "112::1617" + Tag "Stateflow S-Function WBToolboxLibrary_repository 7" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 106 + FunctionName "sf_sfun" + PortCounts "[2 2]" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "TPinv" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "112::1619" + Position [460, 241, 480, 259] + ZOrder 108 } Block { BlockType Outport - Name "Desired Joint Configuration" - SID "1765" - Position [285, 48, 315, 62] - ZOrder 27 + Name "TPinv" + SID "112::5" + Position [460, 101, 480, 119] + ZOrder -6 IconDisplay "Port number" } Line { - ZOrder 1 - SrcBlock "S-Function" + ZOrder 76 + SrcBlock "mat" SrcPort 1 - DstBlock "Desired Joint Configuration" + Points [120, 0] + DstBlock " SFunction " DstPort 1 } Line { - ZOrder 2 - SrcBlock "Desired frame pose" + ZOrder 77 + SrcBlock "tol" SrcPort 1 - DstBlock "S-Function" + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "TPinv" + ZOrder 78 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "TPinv" DstPort 1 } Line { - ZOrder 3 - SrcBlock "Current Joint configuration" + ZOrder 79 + SrcBlock " Demux " SrcPort 1 - DstBlock "S-Function" - DstPort 2 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 80 + 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 SubSystem - Name "wholeBodyStates" - SID "206" - Ports [] - Position [16, 17, 114, 114] - ZOrder -4 - ForegroundColor "white" - DropShadow on - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 275 - $ClassName "Simulink.Mask" - Display "image(imread('wholeBodyStates.png'),'center');" - } - System { - Name "wholeBodyStates" - Location [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "1000" Block { - BlockType SubSystem - Name "Get Limits" - SID "1690" + BlockType S-Function + Name "Yarp Clock" + SID "1773" + Ports [0, 1] + Position [335, 96, 405, 144] + ZOrder 85 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + FunctionName "BlockFactory" + Parameters "'YarpClock','WBToolbox'" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 287 + $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" + RunInitForIconRedraw "off" + } + } + Block { + BlockType S-Function + Name "YarpRead" + SID "1632" Ports [0, 2] - Position [345, 225, 460, 260] - ZOrder 68 - BackgroundColor "[0.513700, 0.851000, 0.670600]" - RequestExecContextInheritance off + Position [90, 94, 150, 141] + ZOrder 22 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + FunctionName "BlockFactory" + Parameters "'YarpRead','WBToolbox',portName,signalSize,blocking,timestamp,autoconnect,errorOnConnection,t" + "imeout" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off Object { $PropName "MaskObject" - $ObjectID 276 + $ObjectID 288 $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 - Max: 1xDoFs vector containing the maximum limit\n - Min: 1xDoFs vector containing the" - " maximum limit\n\nUnit of measurement:\n - Position: rad\n - Velocity: rad/sec" - 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" + Type "YARP Read" + Description "This block behaves as the command 'yarp read'. \n\nBy default it has the option 'Autoconnect' a" + "ctive, which means that the user can only specify the name of the port ('Source Port Name') from which readings a" + "re desired, along with the size of the expected data (e.g. 3 when reading the torso state).\n\nWhen the user unch" + "ecks 'Autoconnect', 'Opened Port Name' field shows up and it corresponds to the name of the port the block will c" + "reate. No autoconnection is performed, and the output will be always zero until the user connects it to some othe" + "r port from command line using 'yarp connect [from] [dest]'.\n\nWhen 'Autoconnect' is off, an additional output c" + "alled 'signal' is added to the block. It outputs 1 if the port has received at least one sample, or 0 otherwise.\n" + Initialization "% Solve error in string parsing. Force the type.\nportName = char(portName);" 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" + 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" + RunInitForIconRedraw "on" Array { Type "Simulink.MaskParameter" - Dimension 2 + Dimension 7 Object { - $ObjectID 277 - Type "popup" - Array { - Type "Cell" - Dimension 2 - Cell "ControlBoard" - Cell "URDF" - PropName "TypeOptions" - } - Name "limitsSource" - Prompt "Limits Source" - Value "ControlBoard" - Evaluate "off" - 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" + $ObjectID 289 + Type "edit" + Name "portName" + Prompt "Source Port Name" + Value "'/portname'" } Object { - $ObjectID 278 - 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" + $ObjectID 290 + Type "edit" + Name "signalSize" + Prompt "Port Size" + Value "1" + } + Object { + $ObjectID 291 + Type "edit" + Name "timeout" + Prompt "Timeout" + Value "0.5" + Visible "off" + } + Object { + $ObjectID 292 + Type "checkbox" + Name "blocking" + Prompt "Blocking Read" + Value "off" + Callback "blocking_val = get_param(gcb, 'blocking');\nmask_visibility = get_param(gcb, 'MaskVisibilities');\n\nif(s" + "trcmp(blocking_val, 'on'))\n mask_visibility{3} = 'on';\n set_param(gcb, 'MaskVisibilities', mask_visibility);" + "\nelse\n mask_visibility{3} = 'off';\n set_param(gcb, 'MaskVisibilities', mask_visibility);\nend\n\nclear bloc" + "king_val mask_visibility" + } + Object { + $ObjectID 293 + Type "checkbox" + Name "timestamp" + Prompt "Read Timestamp" + Value "on" + } + Object { + $ObjectID 294 + Type "checkbox" + Name "autoconnect" + Prompt "Autoconnect" + Value "on" + Callback "autoconnect_val = get_param(gcb, 'Autoconnect');\nprompt_string = get_param(gcb, 'MaskPrompts');\nmask_vi" + "sibilities = get_param(gcb, 'MaskVisibilities');\n\nif(strcmp(autoconnect_val, 'on'))\n prompt_string{1} = 'Sourc" + "e Port Name';\n mask_visibilities{7} = 'on';\n set_param(gcb, 'MaskVisibilities',mask_visibilities);\nelse\n " + " prompt_string{1} = 'Opened Port Name';\n mask_visibilities{7} = 'off';\n set_param(gcb, 'MaskVisibilities',m" + "ask_visibilities);\nend\nset_param(gcb, 'MaskPrompts', prompt_string);\nclear autoconnect_val prompt_string mask_vis" + "ibilities" + } + Object { + $ObjectID 295 + Type "checkbox" + Name "errorOnConnection" + Prompt "Error on missing connection" + Value "on" } PropName "Parameters" } + } + } + Block { + BlockType S-Function + Name "YarpWrite" + SID "1659" + Ports [1] + Position [230, 96, 290, 144] + ZOrder 27 + ForegroundColor "[0.490196, 0.000000, 0.000000]" + FunctionName "BlockFactory" + Parameters "'YarpWrite','WBToolbox',portName,autoconnect,errorOnConnection" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Object { + $PropName "MaskObject" + $ObjectID 296 + $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('YarpWrite')" + RunInitForIconRedraw "off" Array { - Type "Simulink.dialog.Group" + Type "Simulink.MaskParameter" Dimension 3 Object { - $ObjectID 279 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 280 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" + $ObjectID 297 + Type "edit" + Name "portName" + Prompt "Opened Port Name" + Value "'/portname'" } Object { - $ObjectID 281 - Prompt "Simulink:studio:ToolBarParametersMenu" - Array { - Type "Simulink.dialog.parameter.Popup" - Dimension 2 - Object { - $ObjectID 282 - Name "limitsSource" - } - Object { - $ObjectID 283 - Name "limitsType" - } - PropName "DialogControls" - } - Name "ParameterGroupVar" + $ObjectID 298 + 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 284 - Object { - $PropName "DialogControls" - $ObjectID 285 - $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 [832, 225, 3700, 1914] - Open off - PortBlocksUseCompactNotation off - SetDomainSpec off - DomainSpecType "Deduce" - 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 "1000" - 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','WBToolboxLibrary',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 + $ObjectID 299 + Type "checkbox" + Name "errorOnConnection" + Prompt "Error on missing connection" + Value "on" + Visible "off" + } + PropName "Parameters" } } } Block { BlockType SubSystem - Name "Get Measurement" - SID "1671" - Ports [0, 1] - Position [290, 158, 385, 192] - ZOrder 53 - BackgroundColor "[0.513700, 0.851000, 0.670600]" + Name "errors" + SID "714" + Ports [2, 2] + Position [245, 198, 305, 242] + ZOrder -9 + BackgroundColor "[0.300000, 0.580000, 1.000000]" RequestExecContextInheritance off Object { $PropName "MaskObject" - $ObjectID 286 + $ObjectID 300 $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 measure" - "ment.\n\nParameters:\n - Control Mode: The control mode. Choose one of the supplied mode.\n\nUnit of measurement" - ":\n - Position: rad\n - Velocity: rad/sec\n - Acceleration: rad/sec/sec\n - Torque: Nm\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'))" + 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." RunInitForIconRedraw "off" - Object { - $PropName "Parameters" - $ObjectID 287 - $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 288 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 289 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" - } - Object { - $ObjectID 290 - Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 291 - $ClassName "Simulink.dialog.parameter.Popup" - Name "measuredType" - } - Name "ParameterGroupVar" - } - Object { - $ObjectID 292 - Object { - $PropName "DialogControls" - $ObjectID 293 - $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 [832, 225, 3700, 1914] + Name "errors" + Location [0, 29, 1280, 744] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -6199,137 +6027,137 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "1000" + ZoomFactor "100" Block { - BlockType S-Function - Name "S-Function" - SID "1672" - Ports [0, 1] - Position [125, 39, 185, 71] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'GetMeasurement','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName,measuredType" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off + 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 "Estimate" - SID "1673" - Position [210, 48, 240, 62] - ZOrder 25 + 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 "S-Function" + SrcBlock "Add" SrcPort 1 - DstBlock "Estimate" - DstPort 1 - } - } - } - Block { - BlockType SubSystem - Name "Get Motor Measurement" - SID "1796" - Ports [0, 1] - Position [410, 158, 505, 192] - ZOrder 69 - BackgroundColor "[0.960000, 0.770000, 0.460000]" - RequestExecContextInheritance off - Object { - $PropName "MaskObject" - $ObjectID 294 - $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 measure" - "ment.\n\nParameters:\n - Control Mode: The control mode. Choose one of the supplied mode.\n\nUnit of measurement" - ":\n - PWM: duty cycle expressed as percentage [-100, 100]%\n - Current: values in Ampere\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 295 - $ClassName "Simulink.MaskParameter" - Type "popup" - Array { - Type "Cell" - Dimension 7 - Cell "Motor Position" - Cell "Motor Velocity" - Cell "Motor Acceleration" - Cell "Motor PWM" - Cell "Motor Current" - Cell "Motor Back EMF" - Cell "Motor Torque Constant" - PropName "TypeOptions" + Points [10, 0] + Branch { + ZOrder 2 + DstBlock "Divide" + DstPort 1 } - Name "measuredType" - Prompt "Estimate Type" - Value "Motor Current" - Evaluate "off" - Callback "maskStr = get_param(gcb,'measuredType');\n\nswitch maskStr\n case 'Motor Position'\n " - " set_param(gcb,'BackgroundColor', '[0.5137, 0.8510, 0.6706, 1.0]');\n case 'Motor Velocity'\n set_par" - "am(gcb,'BackgroundColor', '[0.5137, 0.6745, 1.0000, 1.0]');\n case 'Motor Acceleration'\n set_param(g" - "cb,'BackgroundColor', '[0.9255, 0.8706, 0.1333, 1.0]');\n case 'Motor PWM'\n set_param(gcb,'Backgroun" - "dColor', '[1, 1, 1, 1.0]');\n case 'Motor Current'\n set_param(gcb,'BackgroundColor', '[0.96, 0.77, 0" - ".46, 1.0]');\n case 'Motor Back EMF'\n set_param(gcb,'BackgroundColor', '[0.8, 0.4, 1.0, 1.0]');\n " - " case 'Motor Torque Constant'\n set_param(gcb,'BackgroundColor', '[0.8, 1.0, 0.4, 1.0]');\n otherwise" - "\n error('Control Type not recognized.')\nend\n\nclear maskStr" - } - Array { - Type "Simulink.dialog.Group" - Dimension 3 - Object { - $ObjectID 296 - Prompt "%" - Object { - $PropName "DialogControls" - $ObjectID 297 - $ClassName "Simulink.dialog.Text" - Prompt "%" - Name "DescTextVar" - } - Name "DescGroupVar" + Branch { + ZOrder 3 + Points [0, -25] + DstBlock "x-y" + DstPort 1 } - Object { - $ObjectID 298 - Prompt "Simulink:studio:ToolBarParametersMenu" - Object { - $PropName "DialogControls" - $ObjectID 299 - $ClassName "Simulink.dialog.parameter.Popup" - Name "measuredType" - } - Name "ParameterGroupVar" + } + 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 } - Object { - $ObjectID 300 - Object { - $PropName "DialogControls" - $ObjectID 301 - $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" + Branch { + ZOrder 7 + Points [60, 0; 0, -50] + DstBlock "Divide" + DstPort 2 } - PropName "DialogControls" } + 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 [115, 197, 175, 243] + ZOrder 14 + BackgroundColor "[0.537255, 0.721569, 1.000000]" + RequestExecContextInheritance off + Object { + $PropName "MaskObject" + $ObjectID 301 + $ClassName "Simulink.Mask" + Type "Holder" + Description "This block holds the first input value during the simulation." + RunInitForIconRedraw "off" } System { - Name "Get Motor Measurement" - Location [832, 225, 3700, 1914] + Name "holder\n" + Location [12, 45, 1340, 980] Open off PortBlocksUseCompactNotation off SetDomainSpec off @@ -6344,35 +6172,207 @@ Library { TiledPaperMargins [0.500000, 0.500000, 0.500000, 0.500000] TiledPageScale 1 ShowPageBoundaries off - ZoomFactor "1000" + ZoomFactor "100" Block { - BlockType S-Function - Name "S-Function" - SID "1797" - Ports [0, 1] - Position [125, 39, 185, 71] - ZOrder 19 - FunctionName "WBToolbox" - Parameters "'GetMeasurement','WBToolboxLibrary',WBTConfigParameters,configBlockAbsName,measuredType" - SFunctionDeploymentMode off - EnableBusSupport off - SFcnIsStateOwnerBlock off + 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.441" + 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" + System { + Name "MATLAB Function" + Location [12, 45, 1135, 3068] + Open off + PortBlocksUseCompactNotation off + SetDomainSpec off + DomainSpecType "Deduce" + 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 "1625" + 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::1624" + Ports [1, 1] + Position [270, 230, 320, 270] + ZOrder 95 + Outputs "1" + } + Block { + BlockType S-Function + Name " SFunction " + SID "1300::1623" + Tag "Stateflow S-Function WBToolboxLibrary_repository 1" + Ports [2, 2] + Position [180, 100, 230, 160] + ZOrder 94 + FunctionName "sf_sfun" + PortCounts "[2 2]" + SFunctionDeploymentMode off + EnableBusSupport off + SFcnIsStateOwnerBlock off + Port { + PortNumber 2 + Name "s0" + } + } + Block { + BlockType Terminator + Name " Terminator " + SID "1300::1625" + Position [460, 241, 480, 259] + ZOrder 96 + } + Block { + BlockType Outport + Name "s0" + SID "1300::25" + Position [460, 101, 480, 119] + ZOrder 11 + IconDisplay "Port number" + } + Line { + ZOrder 76 + SrcBlock "s" + SrcPort 1 + Points [120, 0] + DstBlock " SFunction " + DstPort 1 + } + Line { + ZOrder 77 + SrcBlock "unused" + SrcPort 1 + DstBlock " SFunction " + DstPort 2 + } + Line { + Name "s0" + ZOrder 78 + Labels [0, 0] + SrcBlock " SFunction " + SrcPort 2 + DstBlock "s0" + DstPort 1 + } + Line { + ZOrder 79 + SrcBlock " Demux " + SrcPort 1 + DstBlock " Terminator " + DstPort 1 + } + Line { + ZOrder 80 + SrcBlock " SFunction " + SrcPort 1 + DstBlock " Demux " + DstPort 1 + } + } } Block { BlockType Outport - Name "Estimate" - SID "1798" - Position [210, 48, 240, 62] - ZOrder 25 + Name "s(0)" + SID "1301" + Position [330, 58, 360, 72] + ZOrder 14 IconDisplay "Port number" } Line { ZOrder 1 - SrcBlock "S-Function" + SrcBlock "Clock" SrcPort 1 - DstBlock "Estimate" + 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 + } } } } diff --git a/matlab/library/exported/WBToolboxLibrary.slx b/matlab/library/exported/WBToolboxLibrary.slx index 62fc6f753..50e76ba31 100644 Binary files a/matlab/library/exported/WBToolboxLibrary.slx and b/matlab/library/exported/WBToolboxLibrary.slx differ diff --git a/matlab/slblocks.m b/matlab/slblocks.m index 97931bede..f62a7e71b 100644 --- a/matlab/slblocks.m +++ b/matlab/slblocks.m @@ -6,7 +6,7 @@ % Name of the .mdl file Browser.Library = 'WBToolboxLibrary'; -Browser.Name = 'Whole Body Toolbox'; +Browser.Name = 'WholeBodyToolbox'; Browser.IsFlat = 0; blkStruct.Browser = Browser; diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index c8a37e049..0b47a41c5 100644 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -2,97 +2,17 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -# ================= -# FIND DEPENDENCIES -# ================= +find_package(shlibpp REQUIRED) +find_package(BlockFactory COMPONENTS Core REQUIRED) -find_package(YARP 3.0.0 REQUIRED) - -# Fail if YARP is not compiled as shared library -# see https://github.com/robotology/codyco-modules/issues/44 -if(NOT ${YARP_IS_SHARED_LIBRARY}) - message(FATAL_ERROR "YARP was found, but it was compiled as static library. A shared library version of YARP is required.") -endif() - -find_package(iDynTree 0.7.2 REQUIRED) - -# This find loads Eigen. -# It would be nice using the EigenConfig shipped with eigen package, but -# on Ubuntu 16.04 eigen is old and provides only FindEigen. -find_package(Eigen3 REQUIRED) - -# ==================== -# OPTIONS AND INCLUDES -# ==================== - -# Add configure_block macro -include(Utilities) - -# Settings for RPATH -if(NOT MSVC) - option(WBT_ENABLE_RPATH "Enable RPATH installation" TRUE) - mark_as_advanced(WBT_ENABLE_RPATH) -endif() - -# Export all symbols in Windows -set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) - -# Add a postfix to Windows libraries compiled in debug -if(MSVC) - set(CMAKE_DEBUG_POSTFIX "d") -endif() - -# Control where binaries and libraries are placed in the build folder. -# This simplifies tests running in Windows. -set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") -set(CMAKE_LIBRARY_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") -set(CMAKE_ARCHIVE_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_LIBDIR}") - -include(AddInstallRPATHSupport) -add_install_rpath_support(BIN_DIRS ${CMAKE_INSTALL_PREFIX}/bin - LIB_DIRS ${CMAKE_INSTALL_PREFIX}/mex ${CMAKE_INSTALL_PREFIX}/lib - DEPENDS WBT_ENABLE_RPATH - USE_LINK_PATH) - -# Get include-what-you-see information when compiling -option(WBT_USE_IWYU "Get the output of include-what-you-use" OFF) -if(WBT_USE_IWYU) - find_program(IWYU_PATH NAMES include-what-you-use iwyu) - if(IWYU_PATH) - set(CMAKE_CXX_INCLUDE_WHAT_YOU_USE ${IWYU_PATH}) - endif() -endif() - -include(GNUInstallDirs) -include(InstallBasicPackageFiles) - -# =============== -# TOOLBOX TARGETS -# =============== - -add_subdirectory(core) +add_subdirectory(base) add_subdirectory(library) -# ===================== -# MAIN WBTOOLBOX TARGET -# ===================== - -# Dummy target -add_library(WBToolbox INTERFACE) - -set(WBTOOLBOX_DEPENDENCIES YCM ToolboxCore ToolboxCoder WBToolboxLibrary) -if (WBT_USES_MATLAB) - list(APPEND WBTOOLBOX_DEPENDENCIES ToolboxMex) -endif() - -install( - TARGETS WBToolbox - EXPORT WBToolbox) - install_basic_package_files(WBToolbox VERSION ${PROJECT_VERSION} COMPATIBILITY AnyNewerVersion - EXPORT WBToolbox - DEPENDENCIES ${WBTOOLBOX_DEPENDENCIES} + EXPORT WBToolboxExport + FIRST_TARGET WBToolboxBase + DEPENDENCIES BlockFactoryCore NAMESPACE WBToolbox:: - INCLUDE_FILE ${CMAKE_SOURCE_DIR}/cmake/ExtraPackageConfigVars.cmake.in) + NO_CHECK_REQUIRED_COMPONENTS_MACRO) diff --git a/toolbox/base/CMakeLists.txt b/toolbox/base/CMakeLists.txt new file mode 100644 index 000000000..8eb4c15b2 --- /dev/null +++ b/toolbox/base/CMakeLists.txt @@ -0,0 +1,41 @@ +# Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT). All rights reserved. +# This software may be modified and distributed under the terms of the +# GNU Lesser General Public License v2.1 or any later version. + +set(WBTBASE_PUBLIC_HDR + include/WBToolbox/Base/RobotInterface.h + include/WBToolbox/Base/Configuration.h + include/WBToolbox/Base/WholeBodySingleton.h + include/WBToolbox/Base/WBBlock.h) + +add_library(WBToolboxBase + src/RobotInterface.cpp + src/Configuration.cpp + src/WholeBodySingleton.cpp + src/WBBlock.cpp + ${WBTBASE_PUBLIC_HDR}) + +set_target_properties(WBToolboxBase PROPERTIES + PUBLIC_HEADER "${WBTBASE_PUBLIC_HDR}") + +target_link_libraries(WBToolboxBase + BlockFactory::Core + YARP::YARP_OS + YARP::YARP_init + YARP::YARP_dev + iDynTree::idyntree-model + iDynTree::idyntree-modelio-urdf + iDynTree::idyntree-high-level) + +target_include_directories(WBToolboxBase PUBLIC + $ + $ + $) + +install( + TARGETS WBToolboxBase + EXPORT WBToolboxExport + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/WBToolbox/Base) diff --git a/toolbox/library/include/Base/Configuration.h b/toolbox/base/include/WBToolbox/Base/Configuration.h similarity index 97% rename from toolbox/library/include/Base/Configuration.h rename to toolbox/base/include/WBToolbox/Base/Configuration.h index 17a7b2e9e..70b4592eb 100644 --- a/toolbox/library/include/Base/Configuration.h +++ b/toolbox/base/include/WBToolbox/Base/Configuration.h @@ -15,8 +15,10 @@ #include namespace wbt { - class Configuration; -} + namespace base { + class Configuration; + } // namespace base +} // namespace wbt /** * @brief Store the configuration for whole-body blocks @@ -31,7 +33,7 @@ namespace wbt { * @see wbt::RobotInterface, wbt::WBBlock, wbt::WholeBodySingleton, WBToolbox.Configuration Matlab * Class */ -class wbt::Configuration +class wbt::base::Configuration { private: class impl; diff --git a/toolbox/library/include/Base/RobotInterface.h b/toolbox/base/include/WBToolbox/Base/RobotInterface.h similarity index 60% rename from toolbox/library/include/Base/RobotInterface.h rename to toolbox/base/include/WBToolbox/Base/RobotInterface.h index 2aa9b2116..bfbb8bba9 100644 --- a/toolbox/library/include/Base/RobotInterface.h +++ b/toolbox/base/include/WBToolbox/Base/RobotInterface.h @@ -35,12 +35,14 @@ namespace iDynTree { } namespace wbt { - class RobotInterface; - class Configuration; + namespace base { + class RobotInterface; + class Configuration; - using JointIndex_Yarp = int; - using JointIndex_iDynTree = int; - using JointName = std::string; + using JointIndex_Yarp = int; + using JointIndex_iDynTree = int; + using JointName = std::string; + } // namespace base } // namespace wbt /** @@ -52,7 +54,7 @@ namespace wbt { * * @see wbt::Configuration */ -class wbt::RobotInterface +class wbt::base::RobotInterface { private: class impl; @@ -63,7 +65,7 @@ class wbt::RobotInterface // ======================== RobotInterface() = delete; - RobotInterface(const wbt::Configuration& config); + RobotInterface(const wbt::base::Configuration& config); ~RobotInterface(); // GET METHODS @@ -74,7 +76,7 @@ class wbt::RobotInterface * * @return A reference of the configuration this object refers to. */ - const wbt::Configuration& getConfiguration() const; + const wbt::base::Configuration& getConfiguration() const; /** * @brief Get the object to operate on the configured model @@ -98,28 +100,30 @@ class wbt::RobotInterface // Specialize the getInterface template namespace wbt { - template <> - bool RobotInterface::getInterface(yarp::dev::IControlMode*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IPositionControl*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IPositionDirect*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IVelocityControl*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::ITorqueControl*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IPWMControl*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::ICurrentControl*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IEncoders*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IMotorEncoders*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IControlLimits*& interface); - template <> - bool RobotInterface::getInterface(yarp::dev::IPidControl*& interface); + namespace base { + template <> + bool RobotInterface::getInterface(yarp::dev::IControlMode*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IPositionControl*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IPositionDirect*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IVelocityControl*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::ITorqueControl*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IPWMControl*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::ICurrentControl*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IEncoders*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IMotorEncoders*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IControlLimits*& interface); + template <> + bool RobotInterface::getInterface(yarp::dev::IPidControl*& interface); + } // namespace base } // namespace wbt #endif // WBT_ROBOTINTERFACE_H diff --git a/toolbox/library/include/Base/WBBlock.h b/toolbox/base/include/WBToolbox/Base/WBBlock.h similarity index 67% rename from toolbox/library/include/Base/WBBlock.h rename to toolbox/base/include/WBToolbox/Base/WBBlock.h index 3a0d6284e..8d9fb2361 100644 --- a/toolbox/library/include/Base/WBBlock.h +++ b/toolbox/base/include/WBToolbox/Base/WBBlock.h @@ -9,16 +9,23 @@ #ifndef WBT_WBBLOCK_H #define WBT_WBBLOCK_H -#include "Core/Block.h" +#include #include #include +namespace blockfactory { + namespace core { + class BlockInformation; + class Signal; + } // namespace core +} // namespace blockfactory + namespace wbt { - class WBBlock; - class Signal; - class BlockInformation; - class RobotInterface; - using InputSignalPtr = std::shared_ptr; + namespace base { + class WBBlock; + class RobotInterface; + using InputSignalPtr = std::shared_ptr; + } // namespace base } // namespace wbt namespace iDynTree { @@ -26,12 +33,14 @@ namespace iDynTree { } // namespace iDynTree /** - * @brief Extension of wbt::Block for simplifying the development of whole-body blocks + * @brief Extension of blockfactory::core::Block for simplifying the development of whole-body + * blocks * - * This class provides support of parsing the parameters for creating a wbt::RobotInterface object, - * and helpers for retrieving iDynTree::KinDynComputations and wbt::RobotInterface objects. + * This class provides support of parsing the parameters for creating a wbt::RobotInterface + * object, and helpers for retrieving iDynTree::KinDynComputations and wbt::RobotInterface + * objects. * - * @see wbt::Block + * @see blockfactory::core::block * * @section wbblock_parameters WBBlock Parameters * @@ -47,17 +56,17 @@ namespace iDynTree { * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | 3 | "GravityVector" | * | ::STRING | 1 + Block::NumberOfParameters | 1 | 1 | "ConfBlockName" | * - * @note The first set of parameters are fields of the same struct. For this reason they share the - * same index. + * @note The first set of parameters are fields of the same struct. For this reason they share + * the same index. */ -class wbt::WBBlock : public wbt::Block +class wbt::base::WBBlock : public blockfactory::core::Block { protected: // TODO: pImpl struct iDynTreeRobotState; std::unique_ptr m_robotState; std::string m_confBlockName; - std::shared_ptr m_robotInterface; + std::shared_ptr m_robotInterface; /** * @brief Helper for retrieving the iDynTree::KinDynComputations object @@ -69,7 +78,7 @@ class wbt::WBBlock : public wbt::Block * @brief Helper for retrieving the wbt::RobotInterface object * @return A pointer to wbt::RobotInterface. */ - const std::shared_ptr getRobotInterface() const; + const std::shared_ptr getRobotInterface() const; /** * @brief Helper for setting the robot state inside the iDynTree::KinDynComputations object @@ -83,10 +92,10 @@ class wbt::WBBlock : public wbt::Block * * @see iDynTree::KinDynComputations::setRobotState, wbt::iDynTreeRobotState */ - bool setRobotState(wbt::InputSignalPtr basePose, - wbt::InputSignalPtr jointsPos, - wbt::InputSignalPtr baseVelocity, - wbt::InputSignalPtr jointsVelocity, + bool setRobotState(wbt::base::InputSignalPtr basePose, + wbt::base::InputSignalPtr jointsPos, + wbt::base::InputSignalPtr baseVelocity, + wbt::base::InputSignalPtr jointsVelocity, iDynTree::KinDynComputations* kinDyn); public: @@ -96,9 +105,9 @@ class wbt::WBBlock : public wbt::Block WBBlock(); ~WBBlock() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_WBBLOCK_H diff --git a/toolbox/library/include/Base/WholeBodySingleton.h b/toolbox/base/include/WBToolbox/Base/WholeBodySingleton.h similarity index 90% rename from toolbox/library/include/Base/WholeBodySingleton.h rename to toolbox/base/include/WBToolbox/Base/WholeBodySingleton.h index 84b26a4b9..618d0b2c4 100644 --- a/toolbox/library/include/Base/WholeBodySingleton.h +++ b/toolbox/base/include/WBToolbox/Base/WholeBodySingleton.h @@ -14,12 +14,19 @@ #include namespace wbt { - class WholeBodySingleton; - class RobotInterface; - class Parameters; - class Configuration; + namespace base { + class WholeBodySingleton; + class RobotInterface; + class Configuration; + } // namespace base } // namespace wbt +namespace blockfactory { + namespace core { + class Parameters; + } // namespace core +} // namespace blockfactory + namespace iDynTree { class KinDynComputations; } @@ -34,13 +41,13 @@ namespace iDynTree { * @see wbt::yarpDevices * */ -class wbt::WholeBodySingleton +class wbt::base::WholeBodySingleton { 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; + std::unordered_map> m_interfaces; public: // CONSTRUCTOR / DESTRUCTOR @@ -84,7 +91,7 @@ class wbt::WholeBodySingleton * * @return the singleton instance */ - static wbt::WholeBodySingleton& sharedInstance(); + static wbt::base::WholeBodySingleton& sharedInstance(); /** * Returns the Configuration object labelled by confKey. @@ -145,7 +152,8 @@ class wbt::WholeBodySingleton * @see WholeBodySingleton::storeConfiguration, wbt::Configuration, * Parameters::containConfigurationData */ - std::shared_ptr storeConfiguration(const wbt::Parameters& parameters); + std::shared_ptr + storeConfiguration(const blockfactory::core::Parameters& parameters); /** * Delete the wbt::RobotInterface referred by confKey. No-op if it doesn't exist. diff --git a/toolbox/library/src/Configuration.cpp b/toolbox/base/src/Configuration.cpp similarity index 97% rename from toolbox/library/src/Configuration.cpp rename to toolbox/base/src/Configuration.cpp index 15e901a4f..53e070295 100644 --- a/toolbox/library/src/Configuration.cpp +++ b/toolbox/base/src/Configuration.cpp @@ -6,11 +6,11 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "Base/Configuration.h" +#include "WBToolbox/Base/Configuration.h" #include -using namespace wbt; +using namespace wbt::base; class Configuration::impl { @@ -33,7 +33,7 @@ Configuration::Configuration(const std::string& confKey) pImpl->confKey = confKey; } -Configuration::Configuration(const wbt::Configuration& other) +Configuration::Configuration(const Configuration& other) : pImpl{other.pImpl->clone()} {} diff --git a/toolbox/library/src/RobotInterface.cpp b/toolbox/base/src/RobotInterface.cpp similarity index 91% rename from toolbox/library/src/RobotInterface.cpp rename to toolbox/base/src/RobotInterface.cpp index 61710353a..0f3218138 100644 --- a/toolbox/library/src/RobotInterface.cpp +++ b/toolbox/base/src/RobotInterface.cpp @@ -6,10 +6,10 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "Base/RobotInterface.h" -#include "Base/Configuration.h" -#include "Core/Log.h" +#include "WBToolbox/Base/RobotInterface.h" +#include "WBToolbox/Base/Configuration.h" +#include #include #include #include @@ -30,7 +30,7 @@ #include #include -using namespace wbt; +using namespace wbt::base; // ==================== // ROBOTINTERFACE PIMPL @@ -58,7 +58,7 @@ class RobotInterface::impl std::shared_ptr kinDynComp; YarpInterfaces yarpInterfaces; - const wbt::Configuration config; // Configuration from Simulink Block's parameters + const Configuration config; // Configuration from Simulink Block's parameters impl() = delete; explicit impl(const Configuration& configuration) @@ -81,7 +81,7 @@ class RobotInterface::impl while (!getMeasurement(buffer.data())) { if (++counter == maxIter) { - wbtError << "Failed to get a measurement during the interface initialization."; + bfError << "Failed to get a measurement during the interface initialization."; return false; } // Sleep for some while @@ -119,7 +119,7 @@ class RobotInterface::impl // Fail if the file is not found if (urdf_file_path.empty()) { - wbtError << "ResourceFinder couldn't find urdf file " + urdf_file + "."; + bfError << "ResourceFinder couldn't find urdf file " + urdf_file + "."; return false; } @@ -132,9 +132,9 @@ class RobotInterface::impl // Use ModelLoader to load the reduced model iDynTree::ModelLoader mdlLoader; if (!mdlLoader.loadReducedModelFromFile(urdf_file_path, controlledJoints)) { - wbtError << "Impossible to load " + urdf_file + "." << std::endl - << "Possible causes: file not found, or the joint " - << "list contains an entry not present in the urdf model."; + bfError << "Impossible to load " + urdf_file + "." << std::endl + << "Possible causes: file not found, or the joint " + << "list contains an entry not present in the urdf model."; return false; } @@ -147,7 +147,7 @@ class RobotInterface::impl // Initialize the network yarp::os::Network::init(); if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { - wbtError << "YARP server wasn't found active."; + bfError << "YARP server wasn't found active."; return false; } @@ -187,7 +187,7 @@ class RobotInterface::impl robotDevice = std::unique_ptr(new yarp::dev::PolyDriver()); if (!robotDevice) { - wbtError << "Failed to instantiante an empty PolyDriver class."; + bfError << "Failed to instantiante an empty PolyDriver class."; return false; } @@ -195,7 +195,7 @@ class RobotInterface::impl if (!robotDevice->open(options) && !robotDevice->isValid()) { // Remove garbage if the opening fails robotDevice.reset(); - wbtError << "Failed to open the RemoteControlBoardRemapper with the options passed."; + bfError << "Failed to open the RemoteControlBoardRemapper with the options passed."; return false; } @@ -210,7 +210,7 @@ class RobotInterface::impl // CONSTRUCTOR / DESTRUCTOR // ======================== -RobotInterface::RobotInterface(const wbt::Configuration& config) +RobotInterface::RobotInterface(const Configuration& config) : pImpl{new impl(config)} {} @@ -227,7 +227,7 @@ RobotInterface::~RobotInterface() // GET METHODS // =========== -const wbt::Configuration& RobotInterface::getConfiguration() const +const Configuration& RobotInterface::getConfiguration() const { return pImpl->config; } @@ -240,7 +240,7 @@ const std::shared_ptr RobotInterface::getKinDynCom // Otherwise, initialize a new object if (!pImpl->initializeModel()) { - wbtError << "Failed to initialize the KinDynComputations object."; + bfError << "Failed to initialize the KinDynComputations object."; // Return an empty shared_ptr (implicitly initialized) return nullptr; } @@ -260,13 +260,13 @@ T* RobotInterface::impl::getInterfaceLazyEval( // Lazy-initialize the RemoteControlBoardRemapper device if (!cbRemapper) { if (!initializeRemoteControlBoardRemapper()) { - wbtError << "Failed to initialize the RemoteControlBoardRemapper."; + bfError << "Failed to initialize the RemoteControlBoardRemapper."; return nullptr; } } // Ask the interface from the device if (!robotDevice->view(interface)) { - wbtError << "Failed to view the interface."; + bfError << "Failed to view the interface."; return nullptr; } } diff --git a/toolbox/library/src/WBBlock.cpp b/toolbox/base/src/WBBlock.cpp similarity index 80% rename from toolbox/library/src/WBBlock.cpp rename to toolbox/base/src/WBBlock.cpp index 48d26c596..cb72d6d47 100644 --- a/toolbox/library/src/WBBlock.cpp +++ b/toolbox/base/src/WBBlock.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "Base/WBBlock.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Base/WholeBodySingleton.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Signal.h" - +#include "WBToolbox/Base/WBBlock.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" +#include "WBToolbox/Base/WholeBodySingleton.h" + +#include +#include +#include +#include #include #include #include @@ -29,7 +29,8 @@ #include #include -using namespace wbt; +using namespace wbt::base; +using namespace blockfactory::core; enum ParamIndex { @@ -67,15 +68,15 @@ std::shared_ptr WBBlock::getKinDynComputations() c return m_robotInterface->getKinDynComputations(); } -const std::shared_ptr WBBlock::getRobotInterface() const +const std::shared_ptr WBBlock::getRobotInterface() const { return m_robotInterface; } -bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, - wbt::InputSignalPtr jointsPos, - wbt::InputSignalPtr baseVelocity, - wbt::InputSignalPtr jointsVelocity, +bool WBBlock::setRobotState(InputSignalPtr basePose, + InputSignalPtr jointsPos, + InputSignalPtr baseVelocity, + InputSignalPtr jointsVelocity, iDynTree::KinDynComputations* kinDyn) { // SAVE THE ROBOT STATE @@ -86,7 +87,7 @@ bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, using Matrix4dSimulink = Matrix; if (!m_robotState) { - wbtError << "Failed to access iDynTreeRobotState object."; + bfError << "Failed to access iDynTreeRobotState object."; return false; } @@ -97,7 +98,7 @@ bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, // Get the buffer const double* buffer = basePose->getBuffer(); if (!buffer) { - wbtError << "Failed to read the base pose from input port."; + bfError << "Failed to read the base pose from input port."; return false; } // Fill the data @@ -111,7 +112,7 @@ bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, // Get the buffer const double* buffer = jointsPos->getBuffer(); if (!buffer) { - wbtError << "Failed to read joints positions from input port."; + bfError << "Failed to read joints positions from input port."; return false; } // Fill the data @@ -127,7 +128,7 @@ bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, // Get the buffer const double* buffer = baseVelocity->getBuffer(); if (!buffer) { - wbtError << "Failed to read the base velocity from input port."; + bfError << "Failed to read the base velocity from input port."; return false; } // Fill the data @@ -141,7 +142,7 @@ bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, // Get the buffer const double* buffer = jointsVelocity->getBuffer(); if (!buffer) { - wbtError << "Failed to read joints velocities from input port."; + bfError << "Failed to read joints velocities from input port."; return false; } // Fill the data @@ -154,7 +155,7 @@ bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, // ============================================= if (!kinDyn) { - wbtError << "Failed to access the KinDynComputations object."; + bfError << "Failed to access the KinDynComputations object."; return false; } @@ -165,7 +166,7 @@ bool WBBlock::setRobotState(wbt::InputSignalPtr basePose, m_robotState->gravity); if (!ok) { - wbtError << "Failed to set the iDynTree robot state."; + bfError << "Failed to set the iDynTree robot state."; return false; } @@ -198,7 +199,7 @@ bool WBBlock::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -214,7 +215,7 @@ bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) // Parse the parameters if (!WBBlock::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -228,13 +229,13 @@ bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) m_robotInterface = WholeBodySingleton::sharedInstance().storeConfiguration(m_parameters); if (!m_robotInterface) { - wbtError << "Failed to get the RobotInterface object from the WholeBodySingleton."; + bfError << "Failed to get the RobotInterface object from the WholeBodySingleton."; return false; } // Check if the DoFs are positive if (m_robotInterface->getConfiguration().getNumberOfDoFs() < 1) { - wbtError << "Failed to configure WBBlock. Read 0 DoFs."; + bfError << "Failed to configure WBBlock. Read 0 DoFs."; return false; } @@ -249,7 +250,7 @@ bool WBBlock::initialize(BlockInformation* blockInfo) // Parse the parameters if (!WBBlock::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -259,14 +260,14 @@ bool WBBlock::initialize(BlockInformation* blockInfo) m_robotInterface = WholeBodySingleton::sharedInstance().storeConfiguration(m_parameters); if (!m_robotInterface) { - wbtError << "Failed to get the RobotInterface object from the WholeBodySingleton."; + bfError << "Failed to get the RobotInterface object from the WholeBodySingleton."; return false; } // Check if the DoFs are positive const auto dofs = m_robotInterface->getConfiguration().getNumberOfDoFs(); if (dofs < 1) { - wbtError << "Failed to configure WBBlock. Read 0 DoFs."; + bfError << "Failed to configure WBBlock. Read 0 DoFs."; return false; } @@ -275,7 +276,7 @@ bool WBBlock::initialize(BlockInformation* blockInfo) new iDynTreeRobotState(dofs, m_robotInterface->getConfiguration().getGravityVector())); if (!m_robotState) { - wbtError << "Failed to initialize the iDynTreeRobotState object"; + bfError << "Failed to initialize the iDynTreeRobotState object"; return false; } diff --git a/toolbox/library/src/WholeBodySingleton.cpp b/toolbox/base/src/WholeBodySingleton.cpp similarity index 77% rename from toolbox/library/src/WholeBodySingleton.cpp rename to toolbox/base/src/WholeBodySingleton.cpp index c21893631..c59a71acb 100644 --- a/toolbox/library/src/WholeBodySingleton.cpp +++ b/toolbox/base/src/WholeBodySingleton.cpp @@ -6,11 +6,12 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "Base/WholeBodySingleton.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/Log.h" -#include "Core/Parameters.h" +#include "WBToolbox/Base/WholeBodySingleton.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include #include #include @@ -18,10 +19,11 @@ #include #include -using namespace wbt; +using namespace wbt::base; +using namespace blockfactory::core; -bool fillConfiguration(std::shared_ptr& configurationPtr, - const wbt::Parameters& parameters); +bool fillConfiguration(std::shared_ptr& configurationPtr, + const Parameters& parameters); // CONSTRUCTOR / DESTRUCTOR // ======================== @@ -44,14 +46,13 @@ int WholeBodySingleton::numberOfDoFs(const std::string& confKey) const bool WholeBodySingleton::isKeyValid(const std::string& confKey) const { if (m_interfaces.find(confKey) == m_interfaces.end()) { - wbtError << "Failed to find entry in the ToolboxSingleton related to " << confKey - << " key."; + bfError << "Failed to find entry in the ToolboxSingleton related to " << confKey << " key."; return false; } if (!m_interfaces.at(confKey).lock()) { - wbtError << "Failed to get the RobotInterface object from its weak pointer stored in " - << "TolboxSingleton."; + bfError << "Failed to get the RobotInterface object from its weak pointer stored in " + << "TolboxSingleton."; return false; } @@ -99,7 +100,7 @@ std::shared_ptr WholeBodySingleton::createRobotInterface(const Configuration& config) { if (!config.isValid()) { - wbtError << "The passed configuration object does not contain valid data."; + bfError << "The passed configuration object does not contain valid data."; return {}; } @@ -126,9 +127,9 @@ WholeBodySingleton::createRobotInterface(const Configuration& config) // previously by another block that points to the same Configuration block. Just to be sure, // check if the Configuration objects match: if (!(m_interfaces[confKey].lock()->getConfiguration() == config)) { - wbtError << "A RobotInterface pointing to " << confKey - << " Configuration block already exists, but contains a different " - << "wbt::Configuration object."; + bfError << "A RobotInterface pointing to " << confKey + << " Configuration block already exists, but contains a different " + << "wbt::Configuration object."; return {}; } @@ -137,8 +138,8 @@ WholeBodySingleton::createRobotInterface(const Configuration& config) return m_interfaces[confKey].lock(); } -bool fillConfiguration(std::shared_ptr& configurationPtr, - const wbt::Parameters& parameters) +bool fillConfiguration(std::shared_ptr& configurationPtr, + const Parameters& parameters) { bool ok = true; @@ -159,15 +160,15 @@ bool fillConfiguration(std::shared_ptr& configurationPtr, ok = ok && parameters.getParameter("ConfBlockName", confBlockName); if (!ok) { - wbtError << "The parameters passed do not contain all the required information to create a " - << "Configuration object."; + bfError << "The parameters passed do not contain all the required information to create a " + << "Configuration object."; return false; } // Populate the Configuration object // ================================= - configurationPtr.reset(new wbt::Configuration(confBlockName)); + configurationPtr.reset(new Configuration(confBlockName)); configurationPtr->setRobotName(robotName); configurationPtr->setUrdfFile(urdfFile); configurationPtr->setControlledJoints(controlledJoints); @@ -183,20 +184,19 @@ bool fillConfiguration(std::shared_ptr& configurationPtr, return true; } -std::shared_ptr -WholeBodySingleton::storeConfiguration(const wbt::Parameters& parameters) +std::shared_ptr WholeBodySingleton::storeConfiguration(const Parameters& parameters) { std::shared_ptr configurationPtr = nullptr; // Create the Configuration object. This checks if parameters contain all the required // parameters. if (!fillConfiguration(configurationPtr, parameters)) { - wbtError << "Failed to fill the configuration with input parameters."; + bfError << "Failed to fill the configuration with input parameters."; return {}; } if (!configurationPtr || !configurationPtr->isValid()) { - wbtError << "The parsed Configuration object is not valid."; + bfError << "The parsed Configuration object is not valid."; return {}; } @@ -205,7 +205,7 @@ WholeBodySingleton::storeConfiguration(const wbt::Parameters& parameters) auto robotInterface = createRobotInterface(*configurationPtr); if (!robotInterface) { - wbtError << "Failed to get the RobotInterface object."; + bfError << "Failed to get the RobotInterface object."; return {}; } diff --git a/toolbox/core/CMakeLists.txt b/toolbox/core/CMakeLists.txt deleted file mode 100644 index fc3399b3f..000000000 --- a/toolbox/core/CMakeLists.txt +++ /dev/null @@ -1,149 +0,0 @@ -# Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT). All rights reserved. -# This software may be modified and distributed under the terms of the -# GNU Lesser General Public License v2.1 or any later version. - -# =================== -# CORE SHARED LIBRARY -# =================== - -set(TOOLBOX_CORE_SRC - src/Block.cpp - src/BlockInformation.cpp - src/Log.cpp - src/Parameter.cpp - src/Parameters.cpp - src/ConvertStdVector.cpp - src/Signal.cpp) - -set(TOOLBOX_CORE_HDR - include/Core/Block.h - include/Core/BlockInformation.h - include/Core/Log.h - include/Core/Parameter.h - include/Core/Parameters.h - include/Core/ConvertStdVector.h - include/Core/Signal.h) - -add_library(Core ${TOOLBOX_CORE_SRC} ${TOOLBOX_CORE_HDR}) -add_library(WBToolbox::Core ALIAS Core) - -target_include_directories(Core PUBLIC - $ - $) - -set_target_properties(Core PROPERTIES - # VERSION ${PROJECT_VERSION} - PUBLIC_HEADER "${TOOLBOX_CORE_HDR}" - OUTPUT_NAME "ToolboxCore") - -install( - TARGETS Core - EXPORT ToolboxCoreExport - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/WBToolbox/Core) - -install_basic_package_files(ToolboxCore - VERSION ${PROJECT_VERSION} - COMPATIBILITY AnyNewerVersion - EXPORT ToolboxCoreExport - FIRST_TARGET Core - NAMESPACE WBToolbox:: - NO_CHECK_REQUIRED_COMPONENTS_MACRO) - -# ===================== -# CORE SIMULINK LIBRARY -# ===================== - -if(WBT_USES_MATLAB) - - # Install TLC file - install( - FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolbox.tlc - DESTINATION ${CMAKE_INSTALL_PREFIX}/mex) - - # Compile S-Function - matlab_add_mex( - NAME Mex - OUTPUT_NAME WBToolbox - SRC src/SimulinkBlockInformation.cpp - include/Core/SimulinkBlockInformation.h - src/WBToolbox.cpp - LINK_TO WBToolbox::Core MxAnyType shlibpp::shlibpp) - add_library(WBToolbox::Mex ALIAS Mex) - - target_compile_definitions(Mex PUBLIC "MATLAB_MEX_FILE") - - target_include_directories(Mex PUBLIC - ${Matlab_INCLUDE_DIRS} - ${Matlab_ROOT_DIR}/simulink/include) - - # Install S-Function - install( - TARGETS Mex - EXPORT ToolboxMexExport - DESTINATION mex) - - install_basic_package_files(ToolboxMex - VERSION ${PROJECT_VERSION} - COMPATIBILITY AnyNewerVersion - EXPORT ToolboxMexExport - FIRST_TARGET Mex - DEPENDENCIES ToolboxCore MxAnyType shlibpp - NAMESPACE WBToolbox:: - NO_CHECK_REQUIRED_COMPONENTS_MACRO) -endif() - -# ================== -# CORE CODER LIBRARY -# ================== - -set(CODER_HDR - include/Core/CoderBlockInformation.h - include/Core/GeneratedCodeWrapper.h) - -set(CODER_SRC - src/CoderBlockInformation.cpp) - -add_library(Coder ${CODER_HDR} ${CODER_SRC}) -add_library(WBToolbox::Coder ALIAS Coder) - -set_target_properties(Coder PROPERTIES - # VERSION ${PROJECT_VERSION} - PUBLIC_HEADER "${CODER_HDR}" - OUTPUT_NAME "ToolboxCoder" -) - -target_link_libraries(Coder PUBLIC WBToolbox::Core) -target_include_directories(Coder INTERFACE - $ - $) - -# CMake macro and functions -install( - FILES "${CMAKE_SOURCE_DIR}/cmake/AddGeneratedCodeLib.cmake" - DESTINATION "${CMAKE_INSTALL_DATADIR}/WBToolbox/cmake" -) - -install( - TARGETS Coder - EXPORT ToolboxCoderExport - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/WBToolbox/Core -) - -set(EXTRA_CONTENT - "list(APPEND CMAKE_MODULE_PATH @CMAKE_INSTALL_PREFIX@/share/WBToolbox/cmake)") - -install_basic_package_files(ToolboxCoder - VERSION ${PROJECT_VERSION} - COMPATIBILITY AnyNewerVersion - EXPORT ToolboxCoderExport - FIRST_TARGET Coder - DEPENDENCIES ToolboxCore - NAMESPACE WBToolbox:: - NO_CHECK_REQUIRED_COMPONENTS_MACRO - INCLUDE_CONTENT ${EXTRA_CONTENT}) diff --git a/toolbox/core/WBToolbox.tlc b/toolbox/core/WBToolbox.tlc deleted file mode 100644 index e3da24ed8..000000000 --- a/toolbox/core/WBToolbox.tlc +++ /dev/null @@ -1,272 +0,0 @@ -%% File : WBToolbox.tlc -%% Abstract: Inlined tlc file for S-function WBToolbox.cpp -%% - -%implements "WBToolbox" "C" - -%% Function: NotifyErrors -%% ====================== -%% -%% Abstract: Utility for copying the code used for forwarding -%% errors to the model driver. -%% - -%function NotifyErrors(prefix) Output - - std::string error = wbt::Log::getSingleton().getErrors(); - error = "%" + error; - - // Trim the message if needed - if (error.length() >= 1024) { - error = error.substr(0, 1024-1); - } - - // This shouldn't happen - if (getRTM()->errorStatus) { - delete getRTM()->errorStatus; - getRTM()->errorStatus = nullptr; - } - - getRTM()->errorStatus = new char[1024]; - sprintf(const_cast(getRTM()->errorStatus), "%s", error.c_str()); - return; - %return -%endfunction %% NotifyErrors - -%% Function: Outputs -%% ================= - -%function Outputs(block, system) Output - - %%assign identifier = Identifier - - %% Save the PWork vector locations in TLC variables - %assign PWorkStorage_Block = LibBlockPWork(blockPWork, "", "", 0) - %assign PWorkStorage_BlockInfo = LibBlockPWork(blockPWork, "", "", 1) - - { - // Get the CoderBlockInformation from the PWork - wbt::CoderBlockInformation* blockInfo = nullptr; - blockInfo = static_cast(%); - - // Get the Block from the PWork - wbt::Block* blockPtr = nullptr; - blockPtr = static_cast(%); - - // Calculate the output - // -------------------- - bool ok; - ok = blockPtr->output(blockInfo); - - // Report errors - if (!ok) { - %assign variable = "[Output]" - %assign dummy = NotifyErrors(variable) - } - } - // End of % Block: % -%endfunction %% Outputs - -%% Function: BlockTypeSetup -%% ======================== - -%function BlockTypeSetup(block, system) void - - %")> - %")> - %")> - %")> - %")> - %")> - -%endfunction - -%% Function: BlockInstanceSetup -%% ============================ - -%function BlockInstanceSetup(block, system) void - - %.h>")> - -%endfunction - -%% Function: Start -%% =============== - -%function Start(block, system) Output - - %% Save the PWork vector locations in TLC variables - %assign PWorkStorage_Block = LibBlockPWork(blockPWork, "", "", 0) - %assign PWorkStorage_BlockInfo = LibBlockPWork(blockPWork, "", "", 1) - - { - // Create and store the CoderBlockInformation object - wbt::CoderBlockInformation* blockInfo = new wbt::CoderBlockInformation(); - % = static_cast(blockInfo); - - // Initialize the parameters - // ------------------------- - - wbt::Parameters params; - - %assign numberOfParameters = SFcnParamSettings[0].numberOfParameters - %assign className = SFcnParamSettings[0].className - - %foreach i = numberOfParameters - - %assign i = i + 1 - %assign index = SFcnParamSettings[i].index - %assign name = SFcnParamSettings[i].name - %assign rows = SFcnParamSettings[i].rows - %assign cols = SFcnParamSettings[i].cols - %assign type = SFcnParamSettings[i].type - %assign storage = SFcnParamSettings[i].storage - - %if SFcnParamSettings[i].isScalar == 1.0 - %assign valueScalar = SFcnParamSettings[i].valueScalar - %if storage != "std::string" - params.storeParameter<%>(%, - %else - params.storeParameter<%>("%", - %endif - wbt::ParameterMetadata(wbt::%, %, %, %, "%")); - %else - %assign valueVector = SFcnParamSettings[i].valueVector - { - std::vector<%> valueVector; - %if cols > 0 - valueVector.reserve(%); - %endif - %foreach element = cols - %if storage != "std::string" - valueVector.push_back(%); - %else - valueVector.push_back("%"); - %endif - %endforeach - params.storeParameter<%>(valueVector, - wbt::ParameterMetadata(wbt::%, %, %, %, "%")); - } - %endif - %endforeach - - // Store the parameters in the CoderBlockInformation object - blockInfo->storeRTWParameters(params); - - // Initialize input / output Signals - // --------------------------------- - - %assign numInputPorts = LibBlockNumInputPorts(block) - %assign numOutputPorts = LibBlockNumOutputPorts(block) - - // Inputs - %foreach i = numInputPorts - - %assign dims = LibBlockInputSignalDimensions(i) - %assign numDims = LibBlockInputSignalNumDimensions(i) - %if numDims == 1 - %assign rows = 1 - %assign cols = dims[0] - %else - %assign rows = dims[0] - %assign cols = dims[1] - %endif - %%assign width = LibBlockInputSignalWidth(i) - %assign address = LibBlockInputSignalAddr(i, "", "", 0) - blockInfo->setInputSignal(%, static_cast(%
), {%, %}); - %endforeach - - // Outputs - %foreach i = numOutputPorts - - %assign dims = LibBlockOutputSignalDimensions(i) - %assign numDims = LibBlockOutputSignalNumDimensions(i) - %if numDims == 1 - %assign rows = 1 - %assign cols = dims[0] - %else - %assign rows = dims[0] - %assign cols = dims[1] - %endif - %%assign width = LibBlockOutputSignalWidth(i) - %assign address = LibBlockOutputSignalAddr(i, "", "", 0) - %%blockInfo->setOutputSignal(%, static_cast(%
), %); - blockInfo->setOutputSignal(%, static_cast(%
), {%, %}); - %endforeach - - // Initialize the class - // -------------------- - - // Allocate the block object - wbt::Block* blockPtr = static_cast(new wbt::%()); - - // Run a dummy configureSizeAndPorts step. This is currently required for properly - // handling optional input / outputs static variables. - // TODO: find a better way to handle them. - { - auto tmpCoderBlockInfo = std::unique_ptr( - new wbt::CoderBlockInformation); - tmpCoderBlockInfo->storeRTWParameters(params); - blockPtr->configureSizeAndPorts(tmpCoderBlockInfo.get()); - } - - // Initialize the block - bool ok = blockPtr->initialize(blockInfo); - - // Report errors - if (!ok) { - %assign variable = "[Initialize]" - %assign dummy = NotifyErrors(variable) - } - - // Call the initializeInitialConditions() method - ok = blockPtr->initializeInitialConditions(blockInfo); - - // Report errors - if (!ok) { - %assign variable = "[InitializeInitialConditions]" - %assign dummy = NotifyErrors(variable) - } - - // Store the block in the PWork vector - % = static_cast(blockPtr); - } - // End of % Block: % - -%endfunction %% Start - -%% Function: Terminate -%% =================== - -%function Terminate(block, system) Output - - %% Save the PWork vector locations in TLC variables - %assign PWorkStorage_Block = LibBlockPWork(blockPWork, "", "", 0) - %assign PWorkStorage_BlockInfo = LibBlockPWork(blockPWork, "", "", 1) - - { - // Get the CoderBlockInformation from the PWork - wbt::CoderBlockInformation* blockInfo = nullptr; - blockInfo = static_cast(%); - - // Get the Block from the PWork - wbt::Block* blockPtr = nullptr; - blockPtr = static_cast(%); - - // Terminate the class - // ------------------- - bool ok; - ok = blockPtr->terminate(blockInfo); - - delete blockInfo; - delete blockPtr; - - // Report errors - if (!ok) { - %assign variable = "[Terminate]" - %assign dummy = NotifyErrors(variable) - } - } - // End of % Block: % - -%endfunction %% Terminate diff --git a/toolbox/core/include/Core/Block.h b/toolbox/core/include/Core/Block.h deleted file mode 100644 index 67914f212..000000000 --- a/toolbox/core/include/Core/Block.h +++ /dev/null @@ -1,305 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_BLOCK_H -#define WBT_BLOCK_H - -#include "Core/Parameters.h" -#include -#include - -namespace wbt { - class Block; - class BlockInformation; -} // namespace wbt - -/** - * @brief Basic abstract class for wrapping generic algorithms - * - * ### Rationale - * - * This class is aimed to wrap generic algorithms and it represents the most basic component of - * `WB-Toolbox`. - * - * The entire execution of an generic algorithm can be split in the following major steps: - * - * - Initialization: implemented with Block::configureSizeAndPorts, Block::initialize - * - Execution: implemented with Block::output - * - Termination: implemented with Block::terminate - * - * Considering that the main aim of this toolbox at its beginning was its integration with Simulink, - * it contains other methods for exposing the algorithm as a Simulink block. - * - * ### Main concept - * - * A generic application is the composition of many algorithms sharing and processing each other - * data. Every algorithm is represented by a wbt::Block, and the data shared with other blocks is - * carried by wbt::Signal. - * - * The block is generally unaware of the data that it will process, and it is only interested in - * knowing how many input / output signals are connected, their size and data type. Blocks have the - * concept of _ports_, which are the connections between external wbt::Signal and the block itself. - * - * @remark A signal can be plugged to more than one port. - * - * This kind of information is set by a wbt::BlockInformation object, and it can be specific to the - * framework where the algorithm runs (e.g. standalone C++ code, Simulink, etc). - * - * Beyond this, an algorithm often needs parameters. This class provides functionalities to gather - * them in a wbt::Parameters object. - * - * ### Other information - * - * You can create a new block by deriving this class and implementing at least all the pure virtual - * methods. - * - * @note This class and the entire toolbox assume that algorithms are represented as instantaneous - * systems, there is no default storage capability between different sampling times. However, - * blocks can wrap generic classes which can retain data, outsourcing data persistence outside - * the framework given by `WB-Toolbox`. - * \par - * @note Despite some of the methods inside this class looks Simulink-dependent, objects of this - * class are completely generic. In fact, it only provides algorithm callbacks, and the - * setting of input / output data is demanded to the wbt::BlockInformation interface. For what - * concerns Simulink, a wbt::SimulinkBlockInformation implementation is provided. - * \par - * - * @see wbt::BlockInformation Class for providing information about signals and parameters. - * @see wbt::WBBlock Specialization of wbt::Block which provides useful resources for developing - * whole-body blocks. - * - * @section block_parameters Block Parameters - * - * | Type | Index | Rows | Cols | Name | - * | ---- | :---: | :--: | :--: | ---- | - * | PARAM_STRING | 0 | 1 | 1 | "className" | - * - * @see wbt::ParameterMetadata::ParameterMetadata for the data types of these variables. - */ -class wbt::Block -{ -protected: - /// Container for block's parameter. You can get this member using Block::getParameters - Parameters m_parameters; - -public: - /** - * @brief Create and returns a new Block object of the specified class - * - * If the class does not exist returns `nullptr`. - * - * @param blockClassName The derived class name to be instantiated. - * @return The newly created Block object or `nullptr`. - */ - // static wbt::Block* instantiateBlockWithClassName(const std::string& blockClassName); - - /** - * @brief Destructor - */ - virtual ~Block() = default; - - /** - * Static variable matching Block::numberOfParameters. It might be useful to define parametric - * constants for parameter indices in child blocks. - * - * @see WBBlock::NumberOfParameters - */ - static constexpr unsigned NumberOfParameters = 2; - - /** - * @brief Returns the number of configuration parameters needed by this block - * - * @return The number of parameters. - */ - virtual unsigned numberOfParameters(); - - /** - * @brief Returns the vector of additional block options - * - * Implement this method if you want to store in the Block additional options that can be parsed - * later from BlockInformation::optionFromKey. - * - * @return A vector containing a list of options. - */ - virtual std::vector additionalBlockOptions(); - - /** - * @brief 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 Block::updateDiscreteState - * function. - * @return The number of discrete states. - */ - virtual unsigned numberOfDiscreteStates(); - - /** - * @brief Returns the number of continuous states of the block - * - * The base implementation returns 0, i.e. no continuous states. - * - * @note If you return a number > 0, you should implement the Block::stateDerivative function. - * @return The number of continuous states. - */ - virtual unsigned numberOfContinuousStates(); - - /** - * @brief Update the internal discrete state - * - * i.e. `x[i+1] = f(x[i])` - * - * @param blockInfo A BlockInformation object. - * @return True for success, false otherwise. - */ - virtual bool updateDiscreteState(const BlockInformation* blockInfo); - - /** - * @brief Update the internal continuous state - * - * @param blockInfo The SimStruct structure. - * @return True for success, false otherwise. - */ - virtual bool stateDerivative(const BlockInformation* blockInfo); - - /** - * @brief Specify if the parameter at the specified index is tunable - * - * Tunable means that it can be changed during the simulation. Usually parameters are defined - * before the beginning of the simulation and they stay constant for all its duration. - * - * @note For the time being tunable parameters are not used in this toolbox. - * - * @param[in] index Index of the parameter. - * @param[out] tunable True if the parameter is tunable, false otherwise. - */ - virtual void parameterAtIndexIsTunable(unsigned index, bool& tunable); - - /** - * @brief Parse the parameters stored into the BlockInformation object - * - * Implement this method to create the metadata of the parameters your block needs (using - * wbt::ParameterMetadata), store them into the blockInfo object with - * BlockInformation::addParameterMetadata, and parse them using - * BlockInformation::parseParameters. - * - * An example of the implementation is the following: - * - * ```cpp - * bool Jacobian::parseParameters(BlockInformation* blockInfo) - * { - * ParameterMetadata frameMetadata(PARAM_STRING, PARAM_IDX_FRAME, 1, 1, "frame"); - * bool ok = blockInfo->addParameterMetadata(frameMetadata); - * - * if (!ok) { - * wbtError << "Failed to store parameters metadata."; - * return false; - * } - - * return blockInfo->parseParameters(m_parameters); - * } - * ``` - * - * @param blockInfo The pointer to a BlockInformation object. - * @return True for success, false otherwise. - * @see BlockInformation::addParameterMetadata, BlockInformation::parseParameters - */ - virtual bool parseParameters(BlockInformation* blockInfo); - - /** - * @brief Gather all the stored parameters - * - * After the parameters have been successfully stored and parsed using the - * Block::parseParameter, you can gather them using this method. - * - * @param[out] params A wbt::Parameters object containing block's parameters. - * @return bool True for success, false otherwise. - */ - bool getParameters(wbt::Parameters& params) const; - - /** - * @brief Configure the input and output ports - * - * Implement this method to set information about number and size of input and output signals. - * The terminology `port` comes as Simulink inheritage, and it marks the connection of a signal - * (which resides in some buffer in the program memory) to the block's input or output. - * - * These information can be used later (e.g. in the Block::initialize and Block::output) for - * preallocating resources and accessing data knowing its size in advance. - * - * @note If the size is not known at this stage (Signal::DynamicSize), at latest it should be - * set in the Block::initialize step - * @warning Do not allocate any data in this stage! Object are destroyed afterwards and created - * again before the Block::initialize step. Every allocated memory and stored values - * will be deleted. Only information stored in `blockInfo` will persist. - * - * @param blockInfo The pointer to a BlockInformation object. - * @return True if the block was configured successfully, false otherwise. - * @see BlockInformation::setNumberOfInputPorts, BlockInformation::setInputPortVectorSize - */ - virtual bool configureSizeAndPorts(BlockInformation* blockInfo); - - /** - * Never called. - * - * @param blockInfo The pointer to a BlockInformation object. - * @return True for success, false otherwise. - */ - virtual bool checkParameters(const BlockInformation* blockInfo); - - /** - * @brief Initialize the block - * - * Implement this method to initialize and allocate the resources the algorithm needs during its - * execution. - * - * @param blockInfo The pointer to a BlockInformation object. - * @return True for success, false otherwise. - */ - virtual bool initialize(BlockInformation* blockInfo); - - /** - * @brief Initialize block's initial conditions - * - * Implement this method to specify block's initial conditions. Its execution will happen after - * Block::initialize and before the first call of Block::output. The default is an empty - * implementation. - * - * @note this function is also called on a reset event. In Simulink, an example is when the - * block resides in an enabled subsystem. - * - * @param blockInfo The pointer to a BlockInformation object. - * @return True for success, false otherwise. - */ - virtual bool initializeInitialConditions(const BlockInformation* blockInfo); - - /** - * @brief Cleanup block resources - * - * This method is called during the termination of the execution. Implement this method to - * deallocate all the memory requested during the previous steps or to perform other terminating - * operations. - * - * @param blockInfo The pointer to a BlockInformation object. - * @return True for success, false otherwise. - */ - virtual bool terminate(const BlockInformation* blockInfo); - - /** - * @brief Compute the output of the block - * - * This method is called at every iteration of the model. Implement here a single step of the - * algorithm. - * - * @param blockInfo The pointer to a BlockInformation object. - * @return True for success, false otherwise. - */ - virtual bool output(const BlockInformation* blockInfo) = 0; -}; - -#endif // WBT_BLOCK_H diff --git a/toolbox/core/include/Core/BlockInformation.h b/toolbox/core/include/Core/BlockInformation.h deleted file mode 100644 index b13b578ce..000000000 --- a/toolbox/core/include/Core/BlockInformation.h +++ /dev/null @@ -1,225 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_BLOCKINFORMATION_H -#define WBT_BLOCKINFORMATION_H - -#include -#include -#include -#include -#include - -namespace wbt { - class BlockInformation; - class ParameterMetadata; - class Parameters; - class Configuration; - class RobotInterface; - class Signal; - using InputSignalPtr = std::shared_ptr; - using OutputSignalPtr = std::shared_ptr; - enum class DataType; - // List of possible key for defining block options: - extern const std::string BlockOptionPrioritizeOrder; -} // namespace wbt - -namespace iDynTree { - class KinDynComputations; -} - -/** - * @brief Abstract class for storing generic Block properties - * - * BlockInformation provides an interface for handling implementation-specific properties such as - * input / output number, size and type, number of parameters, ... - * - * A wbt::Block needs to know on what kind of data it operates, and retrieving this information is - * often specific on the framework on top of which blocks run. In order to allow using the same - * Block class from different frameworks (e.g. Simulink, C++, etc), different implementation of this - * interface can be developed to provide a transparent translation of such functionalities. - * - * As an example, take the BlockInformation::parseParameters. In Simulink parameters are read from - * block's masks and Matlab provides a library for reading them. The SimulinkBlockInformation - * implementation will be linked against that library. However, if you want to call the same Block - * class (which is just a wrapper of an algorithm) from a pure C++ main, parameters are read e.g. - * from an xml file. In this case, BlockInformation::parseParameters will parse the xml and fill the - * wbt::Parameters argument. - * - * @see wbt::Block, wbt::Parameters, wbt::Signal - */ -class wbt::BlockInformation -{ -public: - using Rows = int; - using Cols = int; - using PortIndex = int; - - using VectorSize = int; - using MatrixSize = std::pair; - - struct Port - { // The struct provides an enum scope - enum - { - Index = 0, - Dimensions = 1, - DataType = 2, - }; - }; - using PortDimension = std::vector; - using PortData = std::tuple; - - BlockInformation() = default; - virtual ~BlockInformation() = default; - - // ===================== - // BLOCK OPTIONS METHODS - // ===================== - - /** - * @brief Convert a block option from its string identifier to a specific implementation - * - * @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. - */ - virtual bool optionFromKey(const std::string& key, double& option) const = 0; - - // ================== - // PARAMETERS METHODS - // ================== - - /** - * @brief Parse the wbt::Block's parameters - * - * This method allows defining how to gather block's parameters from a specific implementation. - * - * @param[out] parameters A container filled with the parsed parameters. - * @return True for success, false otherwise. - */ - virtual bool parseParameters(wbt::Parameters& parameters) = 0; - - /** - * @brief Add a parameter metadata - * - * In order to parse parameters with BlockInformation::parseParameters, adding in advance their - * metadata can strongly simplify the entire process. - * - * @param paramMD The metadata to add. - * @return True for success, false otherwise. - */ - virtual bool addParameterMetadata(const wbt::ParameterMetadata& paramMD) = 0; - - // ======================== - // PORT INFORMATION SETTERS - // ======================== - - struct IOData; - - /** - * @brief Set input / output ports data - * - * Specify I/O ports data such as BlockInformation::PortIndex, BlockInformation::PortDimension, - * and wbt::DataType storing the information in a BlockInformation::IOData structure. - * - * @param ioData The structure containing I/O ports data. - * @return True for success, false otherwise. - * - * @see setNumberOfInputPorts, setInputPortVectorSize, setInputPortMatrixSize, - * setInputPortDataType - * @note This method automatically sets the number of inputs and outputs. - */ - virtual bool setIOPortsData(const IOData& ioData) = 0; - - // ======================== - // PORT INFORMATION GETTERS - // ======================== - - /** - * @brief Get data of an input port - * - * @param idx The index of the port. - * @return A PortData object containing the port's data. - */ - virtual PortData getInputPortData(PortIndex idx) const = 0; - - /** - * @brief Get data of an output port - * - * @param idx The index of the port. - * @return A PortData object containing the port's data. - */ - virtual PortData getOutputPortData(PortIndex idx) const = 0; - - /** - * @brief Get the size of a 1D input port - * - * @param idx The index of the port. - * @return The size of the port. - */ - virtual VectorSize getInputPortWidth(const PortIndex idx) const = 0; - - /** - * @brief Get the size of a 1D output port - * - * @param idx The index of the port. - * @return The size of the port. - */ - virtual VectorSize getOutputPortWidth(const PortIndex idx) const = 0; - - /** - * @brief Get the size of a 2D input port - * - * @param idx The index of the port. - * @return The size of the port. - */ - virtual MatrixSize getInputPortMatrixSize(const PortIndex idx) const = 0; - - /** - * @brief Get the size of a 2D output port - * - * @param idx The index of the port. - * @return The size of the port. - */ - virtual MatrixSize getOutputPortMatrixSize(const PortIndex idx) const = 0; - - // ============= - // BLOCK SIGNALS - // ============= - - /** - * @brief Get the signal connected to a 1D input port - * - * @param idx The index of the port. - * @param size The size of the signal. - * @return The pointer to the signal connected to the input port for success, a `nullptr` - * otherwise. - */ - virtual wbt::InputSignalPtr getInputPortSignal(const PortIndex idx, - const VectorSize size = -1) const = 0; - - /** - * @brief Get the signal connected to a 1D output port - * - * @param idx The index of the port. - * @param size The size of the signal. - *@return The pointer to the signal connected to the output port for success, a `nullptr` - * otherwise. - */ - virtual wbt::OutputSignalPtr getOutputPortSignal(const PortIndex idx, - const VectorSize size = -1) const = 0; -}; - -struct wbt::BlockInformation::IOData -{ - std::vector input; - std::vector output; -}; - -#endif // WBT_BLOCKINFORMATION_H diff --git a/toolbox/core/include/Core/CoderBlockInformation.h b/toolbox/core/include/Core/CoderBlockInformation.h deleted file mode 100644 index e5cef269b..000000000 --- a/toolbox/core/include/Core/CoderBlockInformation.h +++ /dev/null @@ -1,76 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_CODERBLOCKINFORMATION_H -#define WBT_CODERBLOCKINFORMATION_H - -#include "Core/BlockInformation.h" -#include "Core/Signal.h" - -#include -#include - -namespace wbt { - class CoderBlockInformation; -} - -class wbt::CoderBlockInformation final : public wbt::BlockInformation -{ -private: - class impl; - std::unique_ptr pImpl; - -public: - CoderBlockInformation(); - ~CoderBlockInformation() override; - - // BLOCK OPTIONS METHODS - // ===================== - - bool optionFromKey(const std::string& key, double& option) const override; - - // PARAMETERS METHODS - // ================== - - bool addParameterMetadata(const wbt::ParameterMetadata& paramMD) override; - bool parseParameters(wbt::Parameters& parameters) override; - - // PORT INFORMATION SETTERS - // ======================== - - bool setIOPortsData(const IOData& ioData) override; - - // PORT INFORMATION GETTERS - // ======================== - - PortData getInputPortData(PortIndex idx) const override; - PortData getOutputPortData(PortIndex idx) const override; - VectorSize getInputPortWidth(const PortIndex idx) const override; - VectorSize getOutputPortWidth(const PortIndex idx) const override; - MatrixSize getInputPortMatrixSize(const PortIndex idx) const override; - MatrixSize getOutputPortMatrixSize(const PortIndex idx) const override; - - // BLOCK SIGNALS - // ============= - - wbt::InputSignalPtr - getInputPortSignal(const PortIndex idx, - const VectorSize size = wbt::Signal::DynamicSize) const override; - wbt::OutputSignalPtr - getOutputPortSignal(const PortIndex idx, - const VectorSize size = wbt::Signal::DynamicSize) const override; - - // METHODS OUTSIDE THE INTERFACE - // ============================= - - bool storeRTWParameters(const Parameters& parameters); - bool setInputSignal(const PortIndex idx, void* address, const PortDimension& dims); - bool setOutputSignal(const PortIndex idx, void* address, const PortDimension& dims); -}; - -#endif // WBT_CODERBLOCKINFORMATION_H diff --git a/toolbox/core/include/Core/ConvertStdVector.h b/toolbox/core/include/Core/ConvertStdVector.h deleted file mode 100644 index 6131f9848..000000000 --- a/toolbox/core/include/Core/ConvertStdVector.h +++ /dev/null @@ -1,82 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_CONVERTSTDVECTOR_H -#define WBT_CONVERTSTDVECTOR_H - -#include -#include - -// Details about templates in http://drake.mit.edu/cxx_inl.html - -// Template declaration -// ==================== - -// Its definition will be in the cpp file. This is allowed because all the allowed variant are -// either explicitly declared or explicitly specialized. -namespace wbt { - template - void convertStdVector(const std::vector& input, std::vector& output); -} - -// Explicit declaration for all the other supported types -// ======================================================== - -// Int to other numeric types -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -// Bool to other numeric types -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -// Double to other numeric types -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -extern template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -// String to string -extern template void -wbt::convertStdVector(const std::vector& input, - std::vector& output); - -// Explicit specialization for std::string type -// ============================================ - -namespace wbt { - // String to numeric - template <> - void convertStdVector(const std::vector& input, - std::vector& output); - template <> - void convertStdVector(const std::vector& input, - std::vector& output); - template <> - void convertStdVector(const std::vector& input, - std::vector& output); - // Numeric to string - template <> - void convertStdVector(const std::vector& input, - std::vector& output); - template <> - void convertStdVector(const std::vector& input, - std::vector& output); - template <> - void convertStdVector(const std::vector& input, - std::vector& output); -} // namespace wbt - -#endif diff --git a/toolbox/core/include/Core/GeneratedCodeWrapper.h b/toolbox/core/include/Core/GeneratedCodeWrapper.h deleted file mode 100644 index d5362666a..000000000 --- a/toolbox/core/include/Core/GeneratedCodeWrapper.h +++ /dev/null @@ -1,131 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_GENERATEDCODEWRAPPER_H -#define WBT_GENERATEDCODEWRAPPER_H - -#ifndef MODEL -#error "MODEL option not specified" -#endif - -#include -#include -#include - -namespace wbt { - template - class GeneratedCodeWrapper; -} - -template -class wbt::GeneratedCodeWrapper -{ -private: - std::unique_ptr m_model; - std::string m_modelName; - unsigned m_numSampleTimes; - - bool modelFailed() const; - -public: - GeneratedCodeWrapper(const std::string& modelName = {}, const unsigned& numSampleTimes = 0); - ~GeneratedCodeWrapper() = default; - - bool initialize(); - bool step(); - bool terminate(); - - // double* getOutput(const unsigned& index) const; - - std::string getErrors() const; - // std::string getWarnings() const; -}; - -template -bool wbt::GeneratedCodeWrapper::modelFailed() const -{ - if (m_model) { - if (m_model->getRTM()) { - if (!m_model->getRTM()->errorStatus) - return false; - } - } - return true; -} - -template -wbt::GeneratedCodeWrapper::GeneratedCodeWrapper(const std::string& modelName, - const unsigned& numSampleTimes) - : m_modelName(modelName) - , m_numSampleTimes(numSampleTimes) -{} - -template -bool wbt::GeneratedCodeWrapper::initialize() -{ - if (m_model) { - m_model.reset(); - } - - m_model = std::unique_ptr(new T()); - m_model->initialize(); - - if (modelFailed()) { - return false; - } - - return true; -} - -template -bool wbt::GeneratedCodeWrapper::step() -{ - if (!m_model) { - return false; - } - - m_model->step(); - - if (modelFailed()) { - return false; - } - - return true; -} - -template -bool wbt::GeneratedCodeWrapper::terminate() -{ - if (!m_model) { - return false; - } - - m_model->terminate(); - - if (modelFailed()) { - return false; - } - - return true; -} - -template -std::string wbt::GeneratedCodeWrapper::getErrors() const -{ - if (!m_model) { - return {}; - } - - if (modelFailed()) { - return {m_model->getRTM()->errorStatus}; - } - - return {}; -} - -#endif // WBT_GENERATEDCODEWRAPPER_H diff --git a/toolbox/core/include/Core/Log.h b/toolbox/core/include/Core/Log.h deleted file mode 100644 index 0b77b2e64..000000000 --- a/toolbox/core/include/Core/Log.h +++ /dev/null @@ -1,117 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_LOG_H -#define WBT_LOG_H - -#include -#include -#include - -#ifdef NDEBUG -#define WBT_LOG_VERBOSITY wbt::Log::Verbosity::RELEASE -#else -#define WBT_LOG_VERBOSITY wbt::Log::Verbosity::DEBUG -#endif - -#ifndef wbtError -#define wbtError \ - wbt::Log::getSingleton().getLogStringStream( \ - wbt::Log::Type::ERROR, __FILE__, __LINE__, __FUNCTION__) -#endif - -#ifndef wbtWarning -#define wbtWarning \ - wbt::Log::getSingleton().getLogStringStream( \ - wbt::Log::Type::WARNING, __FILE__, __LINE__, __FUNCTION__) -#endif - -namespace wbt { - class Log; -} // namespace wbt - -/** - * @brief Class for handling log messages - * - * Errors and Warnings are currently supported. - */ -class wbt::Log -{ -public: - enum class Type - { - ERROR, - WARNING - }; - - enum class Verbosity - { - RELEASE, - DEBUG - }; - -private: - class impl; - std::unique_ptr pImpl; - -public: - Log(); - ~Log() = default; - - /** - * @brief Get the Log singleton - * - * There is only one instance in the whole program of this class. - * - * @return The log singleton. - */ - static wbt::Log& getSingleton(); - - /** - * @brief Get the stringstream object for adding log messages - * - * @param type The log type. - * @param file The file from which this method is called (preprocessor directive). - * @param line The line from which this method is called (preprocessor directive). - * @param function The function from which this method is called (preprocessor directive). - * @return The stringstream object matching the log type. - */ - std::stringstream& getLogStringStream(const Type& type, - const std::string& file, - const unsigned& line, - const std::string& function); - - /** - * @brief Get the stored error messages. - * @return The error messages. - */ - std::string getErrors() const; - - /** - * @brief Get the stored warning messages. - * @return The warning messages. - */ - std::string getWarnings() const; - - /** - * @brief Clear the stored error messages. - */ - void clearErrors(); - - /** - * @brief Clear the stored warning messages. - */ - void clearWarnings(); - - /** - * @brief Clear all the stored log messages. - */ - void clear(); -}; - -#endif // WBT_LOG_H diff --git a/toolbox/core/include/Core/Parameter.h b/toolbox/core/include/Core/Parameter.h deleted file mode 100644 index 600d66692..000000000 --- a/toolbox/core/include/Core/Parameter.h +++ /dev/null @@ -1,132 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_PARAMETER_H -#define WBT_PARAMETER_H - -#include -#include - -namespace wbt { - class ParameterMetadata; - template - class Parameter; - enum class ParameterType; -} // namespace wbt - -/** - * @brief Defines the types of parameters supported by wbt::Parameter - * @see wbt::ParameterMetadata, wbt::Parameter - */ -enum class wbt::ParameterType -{ - // Scalar / Vector / Matrix - INT, - BOOL, - DOUBLE, - STRING, - // Cell - CELL_INT, - CELL_BOOL, - CELL_DOUBLE, - CELL_STRING, - // Struct - STRUCT_INT, - STRUCT_BOOL, - STRUCT_DOUBLE, - STRUCT_STRING, - STRUCT_CELL_INT, - STRUCT_CELL_BOOL, - STRUCT_CELL_DOUBLE, - STRUCT_CELL_STRING -}; - -/** - * @brief Class for storing parameter metadata - * - * A metadata must be constructed with an index and a name, and they cannot be changed afterwards. - * Only bool, int, double or std::string types are currently supported. - * - * @see wbt::Parameter, wbt::ParameterType - */ -class wbt::ParameterMetadata -{ -public: - enum - { - DynamicSize = -1 - }; - - const unsigned index; - const std::string name; - - int rows; - int cols; - wbt::ParameterType type; - - ParameterMetadata() = delete; - ~ParameterMetadata() = default; - - ParameterMetadata(const ParameterType& t, - const unsigned& ParamIndex, - const int& paramRows, - const int& paramCols, - const std::string& ParamName = {}); - ParameterMetadata(const ParameterMetadata& paramMD); - ParameterMetadata(ParameterMetadata&& paramMD); - - ParameterMetadata& operator=(const ParameterMetadata& paramMD); - ParameterMetadata& operator=(ParameterMetadata&& paramMD); - bool operator==(const ParameterMetadata& rhs) const; - inline bool operator!=(const ParameterMetadata& rhs) const { return !(*this == rhs); } -}; - -/** - * @brief Class for storing a generic parameter - * - * A generic parameters can be either a scalar or a vector. Supported types are defined by the - * wbt::ParameterType enum.Use wbt::ParameterMetadata to set these information. - * - * @tparam The type of the container type. For vector parameters, T is the type of an element of the - * container. - * @see wbt::Parameters, wbt::ParameterMetadata - */ -template -class wbt::Parameter -{ -private: - using ParamVector = std::vector; - - bool m_isScalar; - T m_valueScalar; - ParamVector m_valueVector; - wbt::ParameterMetadata m_metadata; - -public: - enum class Type; - Parameter() = delete; - ~Parameter() = default; - - Parameter(const T& value, const wbt::ParameterMetadata& md) - : m_isScalar(true) - , m_valueScalar(value) - , m_metadata(md) - {} - Parameter(const ParamVector& valueVec, const wbt::ParameterMetadata& md) - : m_valueVector(valueVec) - , m_isScalar(false) - , m_metadata(md) - {} - - bool isScalar() const { return m_isScalar; } - T getScalarParameter() const { return m_valueScalar; } - ParamVector getVectorParameter() const { return m_valueVector; } - wbt::ParameterMetadata getMetadata() const { return m_metadata; } -}; - -#endif // WBT_PARAMETER_H diff --git a/toolbox/core/include/Core/Parameters.h b/toolbox/core/include/Core/Parameters.h deleted file mode 100644 index 4462d4466..000000000 --- a/toolbox/core/include/Core/Parameters.h +++ /dev/null @@ -1,252 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_PARAMETERS_H -#define WBT_PARAMETERS_H - -#include -#include -#include - -namespace wbt { - template - class Parameter; - class ParameterMetadata; - class Parameters; - const int PARAM_INVALID_INDEX = -1; - const std::string PARAM_INVALID_NAME = {}; -} // namespace wbt - -/** - * @brief Class for storing block's parameters - * - * This class can contain scalar and vector parameters of the supported types. - * - * @see wbt::Parameter, wbt::ParameterMetadata, wbt::ParameterType - */ -class wbt::Parameters -{ -public: - using ParamIndex = int; - using ParamName = std::string; - -private: - class impl; - std::unique_ptr pImpl; - -public: - Parameters(); - Parameters(const wbt::Parameters& other); - Parameters& operator=(const Parameters& other); - - ~Parameters(); - - /** - * @brief Get the name of a stored parameter from its index. - * @param index The index of the parameter. - * @return The name if the parameter exists, wbt::PARAM_INVALID_NAME otherwise. - */ - ParamName getParamName(const ParamIndex& index) const; - - /** - * @brief Get the index of a stored parameter from its name. - * @param name The name of the parameter. - * @return The index if the parameter exists, wbt::PARAM_INVALID_INDEX otherwise. - */ - ParamIndex getParamIndex(const ParamName& name) const; - - /** - * @brief Get the number of stored parameters - * - * @return The number of stored parameters. - */ - unsigned getNumberOfParameters() const; - - /** - * @brief Check if a parameter with a given name is stored - * - * @param name The name of the parameter. - * @return True if the parameters exists, false otherwise. - */ - bool existName(const ParamName& name) const; - - /** - * @brief Store a scalar parameter - * - * @tparam The type of the parameter to store. Despite this, the parameter get cast accordingly - * to its metadata. - * @param param The value of the scalar parameter to store. - * @param paramMetadata The metadata associated to the parameter to store. - * @return True for success, false otherwise. - */ - template - bool storeParameter(const T& param, const wbt::ParameterMetadata& paramMetadata); - - /** - * @brief Store a vector parameter - * - * @tparam The type of the parameter to store. Despite this, the parameter get cast accordingly - * to its metadata. - * @param param The value of the vector parameter to store. - * @param paramMetadata The metadata associated to the parameter to store. - * @return True for success, false otherwise. - */ - template - bool storeParameter(const std::vector& param, const wbt::ParameterMetadata& paramMetadata); - - /** - * @brief Store a parameter - * - * @tparam The type of the parameter to store. - * @param parameter The parameter object to store. - * @return True for success, false otherwise. - * - * @see wbt::Parameter - */ - template - bool storeParameter(const Parameter& parameter); - - /** - * @brief Get a scalar parameter - * - * @tparam The type of the output argument - * @param name The name of the parameter. - * @param[out] param The variable where the parameter value will be stored. Data get cast - * internally, even for string to numeric types. - * @return True for success, false otherwise. - */ - template - bool getParameter(const ParamName& name, T& param) const; - - /** - * @brief Get a vector parameter - * - * @tparam The type of the output argument - * @param name The name of the parameter. - * @param[out] param The variable where the parameter value will be stored. Data get cast - * internally, even for string to numeric types. - * @return True for success, false otherwise. - */ - template - bool getParameter(const ParamName& name, std::vector& param) const; - - /** - * @brief Get all the integer parameters - * - * @return The integer parameters - */ - std::vector> getIntParameters() const; - - /** - * @brief Get all the boolean parameters - * - * @return The boolean parameters - */ - std::vector> getBoolParameters() const; - - /** - * @brief Get all the double parameters - * - * @return The double parameters - */ - std::vector> getDoubleParameters() const; - - /** - * @brief Get all the string parameters - * - * @return The string parameters - */ - std::vector> getStringParameters() const; - - /** - * @brief Get the metadata associated to a stored parameter - * - * @param name The name of the parameter. - * @return The metadata associate with the parameter for success, a metadata with a stored name - * `dummy` otherwise. - */ - wbt::ParameterMetadata getParameterMetadata(const ParamName& name); -}; - -// ============ -// GETPARAMETER -// ============ - -// SCALAR -namespace wbt { - // Explicit declaration for numeric types - extern template bool Parameters::getParameter(const Parameters::ParamName& name, - int& param) const; - extern template bool Parameters::getParameter(const Parameters::ParamName& name, - bool& param) const; - extern template bool Parameters::getParameter(const Parameters::ParamName& name, - double& param) const; - // Explicit specialization for std::string - template <> - bool Parameters::getParameter(const Parameters::ParamName& name, - std::string& param) const; -} // namespace wbt - -// VECTOR -namespace wbt { - // Explicit declaration for numeric types - extern template bool Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; - extern template bool Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; - extern template bool Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; - extern template bool - Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; -} // namespace wbt - -// ============== -// STOREPARAMETER -// ============== - -// SCALAR -namespace wbt { - // Explicit declaration for numeric types - extern template bool Parameters::storeParameter(const int& param, - const ParameterMetadata& paramMetadata); - extern template bool Parameters::storeParameter(const bool& param, - const ParameterMetadata& paramMetadata); - extern template bool Parameters::storeParameter(const double& param, - const ParameterMetadata& paramMetadata); - // Explicit specialization for std::string - template <> - bool Parameters::storeParameter(const std::string& param, - const ParameterMetadata& paramMetadata); -} // namespace wbt - -// VECTOR -namespace wbt { - // Explicit declaration for numeric types - extern template bool Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); - extern template bool Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); - extern template bool Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); - extern template bool - Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); -} // namespace wbt - -// PARAMETER -namespace wbt { - // Explicit declaration for numeric types - extern template bool Parameters::storeParameter(const Parameter& parameter); - extern template bool Parameters::storeParameter(const Parameter& parameter); - extern template bool Parameters::storeParameter(const Parameter& parameter); - extern template bool - Parameters::storeParameter(const Parameter& parameter); -} // namespace wbt - -#endif // WBT_PARAMETERS_H diff --git a/toolbox/core/include/Core/Signal.h b/toolbox/core/include/Core/Signal.h deleted file mode 100644 index 327209d8c..000000000 --- a/toolbox/core/include/Core/Signal.h +++ /dev/null @@ -1,268 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_SIGNAL_H -#define WBT_SIGNAL_H - -#include - -namespace wbt { - class Signal; - enum class DataType; -} // namespace wbt - -/** - * @brief Defines allowed signal data types - * - * This enum defines the data types of signals that are handled by this toolbox. - * - * @note Currently only `DOUBLE` is fully implemented. - * @see Signal::Signal, BlockInformation::setInputPortType, BlockInformation::setOutputPortType - */ -enum class wbt::DataType -{ - DOUBLE, - SINGLE, - INT8, - UINT8, - INT16, - UINT16, - INT32, - UINT32, - BOOLEAN, -}; - -/** - * @brief Class to represent data shared between blocks, labelled as signals. - * - * Analogously to the block-algorithm corrispondence, this class introduces the signal-data - * corrispondence. Signals are basically the connections between blocks. - * - * Signals do not directly translate to block's input and output. Signals are plugged to block - * ports, and this block port fill the signal with data. - * - * @remark A signal can be plugged to more than one block port. - * @see wbt::Block - */ -class wbt::Signal -{ -public: - /// Defines the format of signals supported by Signal. It specifies what kind of data the - /// Signal::m_bufferPtr points. - /// - /// - `NONCONTIGUOUS` matches the default Simulink input signals. Signal::m_bufferPtr is a - /// pointer to an array of pointers, each of them storing an element of the signal. - /// - `CONTIGUOUS` means that the Signal::m_bufferPtr points to a contiguous array of - /// Signal::m_portDataType type. - /// - `CONTIGUOUS_ZEROCOPY` matches the default Simulink output signals. Signal::m_bufferPtr is - /// a pointer to an _external_ array of Signal::m_portDataType type. - /// - /// @note `CONTIGUOUS_ZEROCOPY` is the only format that doesn't copy data from the original - /// buffer address. Instead, both `CONTIGUOUS` and `NONCONTIGUOUS` data formats copy - /// the content of the buffer inside the Signal object. For performance reason, prefer - /// using `CONTIGUOUS_ZEROCOPY`. - /// @see initializeBufferFromContiguous, initializeBufferFromContiguousZeroCopy, - /// initializeBufferFromNonContiguous - /// @see BlockInformation::getInputPortSignal, BlockInformation::getOutputPortSignal - enum class DataFormat - { - NONCONTIGUOUS = 0, - CONTIGUOUS = 1, - CONTIGUOUS_ZEROCOPY = 2 - }; - -private: - class impl; - std::unique_ptr pImpl; - -public: - enum - { - DynamicSize = -1 - }; - - Signal(const DataFormat& dataFormat = DataFormat::CONTIGUOUS_ZEROCOPY, - const DataType& dataType = DataType::DOUBLE); - ~Signal(); - - Signal(const Signal& other); - Signal& operator=(const Signal& other) = delete; - - Signal(Signal&& other); - Signal& operator=(Signal&& other) = delete; - - /** - * @brief Initialize the signal from a contiguous buffer - * - * This method allocates a new array with the same size of `buffer` and copies the data. In this - * case, the Signal object will own the data. - * - * @param buffer The pointer to the original contiguous buffer. - * @return True for success, false otherwise. - * @see Signal::DataFormat - */ - bool initializeBufferFromContiguous(const void* buffer); - - /** - * @brief Initialize the signal from a contiguous buffer without copying data - * - * This methods accepts an external contiguous buffer and holds its pointer. The data is not - * owned by this object. - * - * @note You must set the signal width with Signal::setWidth in order to have a valid signal. - * - * @param buffer The pointer to the original contiguous buffer. - * @return True for success, false otherwise. - * @see Signal::DataFormat - */ - bool initializeBufferFromContiguousZeroCopy(const void* buffer); - - /** - * @brief Initialize the signal from a non-contiguous buffer - * - * This method allocates a new array with the same size of `bufferPtrs` and copies the data. In - * this case, the Signal object will own the data. `bufferPtrs` points to an array of pointers. - * Each of these pointers points to a data element. - * - * @note You must set the signal width with Signal::setWidth in order to have a valid signal. - * - * @param bufferPtrs The pointer to the original non-contiguous buffer. - * @return True for success, false otherwise. - * @see Signal::DataFormat - */ - bool initializeBufferFromNonContiguous(const void* const* bufferPtrs); - - /** - * @brief Check if the signal is valid - * - * Checks if Signal::m_bufferPtr is not `nullptr` and Signal::m_width is greater than zero. - * - * @return True for valid signal, false otherwise. - */ - bool isValid() const; - - /** - * @brief Read the width of the signal - * - * By default the width of Signal is Signal::DynamicSize. However, for being a valid signal, an - * object must have a specified width. - * - * @return The signal width. - * @see Signal::setWidth, Signal::isValid - */ - int getWidth() const; - - /** - * @brief Read the wbt::DataType of the signal - * - * The default type is DataType::DOUBLE. - * - * @return The signal data type. - */ - DataType getPortDataType() const; - - /** - * @brief Read the Signal::DataFormat of the signal - * - * The default type is DataFormat::CONTIGUOUS_ZEROCOPY. - * - * @return The signal data format. - */ - DataFormat getDataFormat() const; - - /** - * @brief Get the pointer to the buffer storing signal's data - * - * The buffer is stored as a void pointer in Signal::m_bufferPtr. In order to use the buffer it - * should be properly cast to the right data type. Be sure that the Signal::DataType match the - * type of the buffer otherwise pointer arithmetics does not work. - * - * If `T` does not match Signal::m_portDataType the returned value is a `nullptr`. - * - * @note Always check if the pointer is not `nullptr` before using it. - * @tparam The data type of the returned buffer. - * @return The pointer to the buffer if the class is properly configured, `nullptr` otherwise. - * @see Signal::setBuffer - */ - template - T* getBuffer(); - - /** - * @brief Get the pointer to the buffer storing signal's data - * - * Documented in Signal::getBuffer - * - */ - template - const T* getBuffer() const; - - /** - * @brief Get a single element of the signal - * - * This method returns the `i-th` element of the handled buffer. - * - * @note It is recommended to use Signal::isValid before using this method. - * @tparam The data type of the returned signal. It must match Signal::m_portDataType. - * @param i The index of the element. It should not exceed Signal::m_width. - * @return The `i-th` element of the vector if the signal is valid, or the default value of the - * type otherwise. - */ - template - T get(const unsigned i) const; - - /** - * @brief Set the width of the signal - * - * @param width The width to set. - */ - void setWidth(const unsigned width); - - /** - * @brief Set the value of a sigle element of the buffer - * - * @param index The index of the element to write. - * @param data The content of the data to write. - * @return True for success, false otherwise. - * - * @todo Port this to a template - */ - bool set(const unsigned index, const double data); - - /** - * @brief Set the pointer to the buffer storing signal's data - * - * This method allows changing the handled buffer. In the DataFormat::CONTIGUOUS case the data - * is copied inside the object. Instead, in the DataFormat::CONTIGUOUS_ZEROCOPY only the pointer - * to the buffer is changed. - * - * This method is not allowed for DataFormat::NONCONTIGUOUS format. - * - * @tparam The data type of the new buffer. - * @param data The new buffer address. - * @param length The size of the new buffer. - * @return True if the buffer was set sucessfully, false otherwise. - */ - template - bool setBuffer(const T* data, const unsigned length); -}; - -// Explicit declaration of templates for all the supported types -// ============================================================= - -// TODO: for the time being, only DOUBLE is allowed. The toolbox has an almost complete support to -// many other data types, but they need to be tested. - -namespace wbt { - // DataType::DOUBLE - extern template double* Signal::getBuffer(); - extern template const double* Signal::getBuffer() const; - extern template double Signal::get(const unsigned i) const; - extern template bool Signal::setBuffer(const double* data, const unsigned length); -} // namespace wbt - -#endif // WBT_SIGNAL_H diff --git a/toolbox/core/include/Core/SimulinkBlockInformation.h b/toolbox/core/include/Core/SimulinkBlockInformation.h deleted file mode 100644 index 2de495cdc..000000000 --- a/toolbox/core/include/Core/SimulinkBlockInformation.h +++ /dev/null @@ -1,120 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_SIMULINKBLOCKINFORMATION_H -#define WBT_SIMULINKBLOCKINFORMATION_H - -#include "AnyType.h" -#include "BlockInformation.h" -#include "Parameter.h" -#include "Signal.h" - -#include -#include -#include - -namespace wbt { - class SimulinkBlockInformation; -} // namespace wbt - -class wbt::SimulinkBlockInformation final : public wbt::BlockInformation -{ -private: - SimStruct* simstruct; - std::string m_confBlockName; - std::vector m_paramsMetadata; - - wbt::DataType mapSimulinkToPortType(const DTypeId typeId) const; - DTypeId mapPortTypeToSimulink(const wbt::DataType dataType) const; - -public: - using ParameterIndex = unsigned; - - SimulinkBlockInformation(SimStruct* simstruct); - ~SimulinkBlockInformation() override = default; - - // BLOCK OPTIONS METHODS - // ===================== - - bool optionFromKey(const std::string& key, double& option) const override; - - // PARAMETERS METHODS - // ================== - - bool addParameterMetadata(const wbt::ParameterMetadata& paramMD) override; - bool parseParameters(wbt::Parameters& parameters) override; - - // PORT INFORMATION SETTERS - // ======================== - - bool setIOPortsData(const IOData& ioData) override; - - // PORT INFORMATION GETTERS - // ======================== - - PortData getInputPortData(const PortIndex idx) const override; - PortData getOutputPortData(const PortIndex idx) const override; - VectorSize getInputPortWidth(const PortIndex idx) const override; - VectorSize getOutputPortWidth(const PortIndex idx) const override; - MatrixSize getInputPortMatrixSize(const PortIndex idx) const override; - MatrixSize getOutputPortMatrixSize(const PortIndex idx) const override; - - // BLOCK SIGNALS - // ============= - - wbt::InputSignalPtr - getInputPortSignal(const PortIndex idx, - const VectorSize size = wbt::Signal::DynamicSize) const override; - wbt::OutputSignalPtr - getOutputPortSignal(const PortIndex idx, - const VectorSize size = wbt::Signal::DynamicSize) const override; - - // METHODS OUTSIDE THE INTERFACE - // ============================= - - // Ports methods - bool updateInputPortData(const PortData& portData); - bool updateOutputPortData(const PortData& portData); - bool setNumberOfInputPorts(const unsigned numberOfPorts); - bool setNumberOfOutputPorts(const unsigned numberOfPorts); - bool setInputPortType(const PortIndex idx, const wbt::DataType type); - bool setOutputPortType(const PortIndex idx, const wbt::DataType type); - bool setInputPortVectorSize(const PortIndex idx, const VectorSize& size); - bool setInputPortMatrixSize(const PortIndex idx, const MatrixSize& size); - bool setOutputPortVectorSize(const PortIndex idx, const VectorSize& size); - bool setOutputPortMatrixSize(const PortIndex idx, const MatrixSize& size); - - // Scalar parameters - bool getScalarParameterAtIndex(const ParameterIndex idx, double& value) const; - bool getBooleanParameterAtIndex(const ParameterIndex idx, bool& value) const; - bool getStringParameterAtIndex(const ParameterIndex idx, std::string& value) const; - - // Struct parameters - bool getStringFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - std::string& value) const; - bool getScalarFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - double& value) const; - bool getBooleanFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - bool& value) const; - bool getCellFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - AnyCell& value) const; - bool getVectorDoubleFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - std::vector& value) const; - - // Cell / Struct / Vector paramters - bool getCellAtIndex(const ParameterIndex idx, AnyCell& value) const; - bool getStructAtIndex(const ParameterIndex idx, AnyStruct& value) const; - bool getVectorAtIndex(const ParameterIndex idx, std::vector& value) const; -}; - -#endif /* WBT_SIMULINKBLOCKINFORMATION_H */ diff --git a/toolbox/core/src/Block.cpp b/toolbox/core/src/Block.cpp deleted file mode 100644 index dc86084e6..000000000 --- a/toolbox/core/src/Block.cpp +++ /dev/null @@ -1,103 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/Block.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" - -#include - -using namespace wbt; - -unsigned Block::numberOfParameters() -{ - return Block::NumberOfParameters; -} - -std::vector Block::additionalBlockOptions() -{ - return {}; -} - -void Block::parameterAtIndexIsTunable(unsigned /*index*/, bool& tunable) -{ - tunable = false; -} - -bool Block::checkParameters(const BlockInformation* /*blockInfo*/) -{ - return true; -} - -bool Block::parseParameters(BlockInformation* blockInfo) -{ - if (!blockInfo->addParameterMetadata({ParameterType::STRING, 0, 1, 1, "className"}) - || !blockInfo->addParameterMetadata({ParameterType::STRING, 1, 1, 1, "libName"})) { - wbtError << "Failed to add parameters metadata."; - return false; - } - - return blockInfo->parseParameters(m_parameters); -} - -bool Block::configureSizeAndPorts(BlockInformation* blockInfo) -{ - if (!Block::parseParameters(blockInfo)) { - wbtError << "Failed to parse Block parameters."; - return false; - } - - return true; -} - -bool Block::initialize(BlockInformation* blockInfo) -{ - if (!Block::parseParameters(blockInfo)) { - wbtError << "Failed to parse Block parameters."; - return false; - } - - return true; -} - -unsigned Block::numberOfDiscreteStates() -{ - return 0; -} - -unsigned Block::numberOfContinuousStates() -{ - return 0; -} - -bool Block::updateDiscreteState(const BlockInformation* /*blockInfo*/) -{ - return true; -} - -bool Block::stateDerivative(const BlockInformation* /*blockInfo*/) -{ - return true; -} - -bool Block::initializeInitialConditions(const BlockInformation* /*blockInfo*/) -{ - return true; -} - -bool Block::terminate(const BlockInformation* /*blockInfo*/) -{ - return true; -} - -bool Block::getParameters(wbt::Parameters& params) const -{ - params = m_parameters; - return true; -} diff --git a/toolbox/core/src/BlockInformation.cpp b/toolbox/core/src/BlockInformation.cpp deleted file mode 100644 index ce529faf0..000000000 --- a/toolbox/core/src/BlockInformation.cpp +++ /dev/null @@ -1,11 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/BlockInformation.h" - -const std::string wbt::BlockOptionPrioritizeOrder = "wbt.BlockOptionPrioritizeOrder"; diff --git a/toolbox/core/src/CoderBlockInformation.cpp b/toolbox/core/src/CoderBlockInformation.cpp deleted file mode 100644 index 3dc85b99f..000000000 --- a/toolbox/core/src/CoderBlockInformation.cpp +++ /dev/null @@ -1,337 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/CoderBlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" - -#include -#include -#include -#include -#include -#include - -using namespace wbt; - -class CoderBlockInformation::impl -{ -public: - unsigned numberOfInputs; - unsigned numberOfOutputs; - - std::vector paramsMetadata; - std::unordered_map> inputSignals; - std::unordered_map> outputSignals; - - std::string confBlockName; - Parameters parametersFromRTW; - - std::unordered_map - inputPortDimensions; - std::unordered_map - outputPortDimensions; -}; - -CoderBlockInformation::CoderBlockInformation() - : pImpl{new CoderBlockInformation::impl()} -{} - -CoderBlockInformation::~CoderBlockInformation() = default; - -// BLOCK OPTIONS METHODS -// ===================== - -bool CoderBlockInformation::optionFromKey(const std::string& /*key*/, double& /*option*/) const -{ - return true; -} - -// PORT INFORMATION SETTERS -// ======================== - -bool CoderBlockInformation::setIOPortsData(const BlockInformation::IOData& /*ioData*/) -{ - // This method is called only in the Block::configureSizeAndPorts method, which is never called - // in the Simulink Coder pipeline. - return false; -} - -// PORT INFORMATION GETTERS -// ======================== - -BlockInformation::VectorSize CoderBlockInformation::getInputPortWidth(const PortIndex idx) const -{ - if (pImpl->inputPortDimensions.find(idx) == pImpl->inputPortDimensions.end()) { - wbtError << "Failed to get width of signal at index " << idx << "."; - return 0; - } - - // mdlRTW writes always a {rows, cols} structure, and vectors are row vectors. - // This means that their dimension is the cols entry. - return pImpl->inputPortDimensions.at(idx).at(1); -} - -BlockInformation::VectorSize CoderBlockInformation::getOutputPortWidth(const PortIndex idx) const -{ - if (pImpl->outputPortDimensions.find(idx) == pImpl->outputPortDimensions.end()) { - wbtError << "Failed to get width of signal at index " << idx << "."; - return 0; - } - - // mdlRTW writes always a {rows, cols} structure, and vectors are row vectors. - // This means that their dimension is the cols entry. - return pImpl->outputPortDimensions.at(idx).at(1); -} - -wbt::InputSignalPtr CoderBlockInformation::getInputPortSignal(const PortIndex idx, - const VectorSize size) const -{ - if (pImpl->inputSignals.find(idx) == pImpl->inputSignals.end()) { - wbtError << "Trying to get non-existing signal " << idx << "."; - return {}; - } - - // TODO: portWidth is used only if the signal is dynamically sized. In Simulink, in this case - // the size is gathered from the SimStruct. From the coder instead? Is it possible having - // a signal with dynamic size in the rtw file?? - // TODO: is it better this check or the one implemented in getOutputPortSignal? - if (size != Signal::DynamicSize && pImpl->inputSignals.at(idx)->getWidth() != size) { - wbtError << "Signals with dynamic sizes (index " << idx - << ") are not supported by the CoderBlockInformation."; - return {}; - } - - if (!pImpl->inputSignals.at(idx)->isValid()) { - wbtError << "Input signal at index " << idx << " is not valid."; - return {}; - } - - return pImpl->inputSignals.at(idx); -} - -wbt::OutputSignalPtr CoderBlockInformation::getOutputPortSignal(const PortIndex idx, - const VectorSize /*size*/) const -{ - if (pImpl->outputSignals.find(idx) == pImpl->outputSignals.end()) { - wbtError << "Trying to get non-existing signal " << idx << "."; - return {}; - } - - if (pImpl->outputSignals.at(idx)->getWidth() == Signal::DynamicSize) { - wbtError << "Signals with dynamic sizes (index " << idx - << ") are not supported by the CoderBlockInformation."; - return {}; - } - - if (!pImpl->outputSignals.at(idx)->isValid()) { - wbtError << "Output signal at index " << idx << " is not valid."; - return {}; - } - - return pImpl->outputSignals.at(idx); -} - -BlockInformation::MatrixSize -CoderBlockInformation::getInputPortMatrixSize(const BlockInformation::PortIndex idx) const -{ - if (pImpl->inputPortDimensions.find(idx) == pImpl->inputPortDimensions.end()) { - wbtError << "Trying to get the size of non-existing signal " << idx << "."; - return {}; - } - - return {pImpl->inputPortDimensions.at(idx)[0], pImpl->inputPortDimensions.at(idx)[1]}; -} - -BlockInformation::MatrixSize -CoderBlockInformation::getOutputPortMatrixSize(const BlockInformation::PortIndex idx) const -{ - if (pImpl->outputPortDimensions.find(idx) == pImpl->outputPortDimensions.end()) { - wbtError << "Trying to get the size of non-existing signal " << idx << "."; - return {}; - } - - return {pImpl->outputPortDimensions.at(idx)[0], pImpl->outputPortDimensions.at(idx)[1]}; -} - -bool CoderBlockInformation::addParameterMetadata(const wbt::ParameterMetadata& paramMD) -{ - for (auto md : pImpl->paramsMetadata) { - if (md.name == paramMD.name) { - wbtError << "Trying to store an already existing " << md.name << " parameter."; - return false; - } - } - - pImpl->paramsMetadata.push_back(paramMD); - return true; -} - -// PARAMETERS METHODS -// ================== - -bool CoderBlockInformation::parseParameters(wbt::Parameters& parameters) -{ - if (pImpl->parametersFromRTW.getNumberOfParameters() == 0) { - wbtError << "The Parameters object containing the parameters to parse is empty."; - return false; - } - - for (wbt::ParameterMetadata& md : pImpl->paramsMetadata) { - // Check that all the parameters that are parsed have already been stored from the coder - if (!pImpl->parametersFromRTW.existName(md.name)) { - wbtError << "Trying to get a parameter value for " << md.name - << ", but its value has never been stored."; - return false; - } - - // Handle the case of dynamically sized columns. In this case the metadata passed - // from the Block (containing DynamicSize) is modified with the length of the - // vector that is going to be stored. - if (md.cols == ParameterMetadata::DynamicSize) { - const auto colsFromRTW = pImpl->parametersFromRTW.getParameterMetadata(md.name).cols; - if (colsFromRTW == ParameterMetadata::DynamicSize) { - wbtError << "Trying to store the cols of a dynamically sized parameters, but the " - << "metadata does not specify a valid size. Probably the block didn't " - << "updat the size in its initialization phase."; - return false; - } - md.cols = colsFromRTW; - } - - if (md != pImpl->parametersFromRTW.getParameterMetadata(md.name)) { - wbtError << "Trying to parse a parameter which metadata differs from the metadata " - << "stored by Simulink Coder."; - return false; - } - } - - // This implementation of BlockInformation contains all the parameters from the very beginning, - // stored using the storeRTWParameters method. Here for simplicity all the stored parameters are - // returned, even if the metadata contain only a subset of them. - parameters = pImpl->parametersFromRTW; - return true; -} - -BlockInformation::PortData -CoderBlockInformation::getInputPortData(BlockInformation::PortIndex idx) const -{ - // TODO: hardcoded DataType::DOUBLE. - return std::make_tuple(idx, pImpl->inputPortDimensions.at(idx), DataType::DOUBLE); -} - -BlockInformation::PortData -CoderBlockInformation::getOutputPortData(BlockInformation::PortIndex idx) const -{ - // TODO: hardcoded DataType::DOUBLE. - return std::make_tuple(idx, pImpl->outputPortDimensions.at(idx), DataType::DOUBLE); -} - -bool CoderBlockInformation::storeRTWParameters(const Parameters& parameters) -{ - if (parameters.getNumberOfParameters() == 0) { - wbtError << "The Parameters object passed doesn't contain any parameter."; - return false; - } - - pImpl->parametersFromRTW = parameters; - return true; -} - -bool CoderBlockInformation::setInputSignal(const PortIndex portNumber, - void* address, - const PortDimension& dims) -{ - if ((pImpl->inputSignals.find(portNumber) != pImpl->inputSignals.end()) - || (pImpl->inputPortDimensions.find(portNumber) != pImpl->inputPortDimensions.end())) { - wbtError << "The signal " << portNumber << "has already been previously stored."; - return false; - } - - if (!address) { - wbtError << "The pointer to the signal to store is a nullptr."; - return false; - } - - if (dims.size() > 2) { - wbtError << "Signal with more than 2 dimensions are not currently supported."; - return false; - } - - // Store the input signal - // TODO: hardcoded DataType::DOUBLE - pImpl->inputSignals.insert( - {portNumber, - std::make_shared(Signal::DataFormat::CONTIGUOUS_ZEROCOPY, DataType::DOUBLE)}); - - // Compute the width of the signal - unsigned numElements = 1; - for (auto dimension : dims) { - numElements *= dimension; - } - - // Configure the signal - pImpl->inputSignals[portNumber]->setWidth(numElements); - if (!pImpl->inputSignals[portNumber]->initializeBufferFromContiguousZeroCopy(address)) { - wbtError << "Failed to configure buffer for input signal connected to the port with index " - << portNumber << "."; - return false; - } - - // Store the dimensions in the map - pImpl->inputPortDimensions.emplace(portNumber, dims); - - return true; -} - -bool CoderBlockInformation::setOutputSignal(const PortIndex portNumber, - void* address, - const PortDimension& dims) -{ - if ((pImpl->outputSignals.find(portNumber) != pImpl->outputSignals.end()) - || (pImpl->outputPortDimensions.find(portNumber) != pImpl->outputPortDimensions.end())) { - wbtError << "The signal " << portNumber << "has already been previously stored."; - return false; - } - - if (!address) { - wbtError << "The pointer to the signal to store is a nullptr."; - return false; - } - - if (dims.size() > 2) { - wbtError << "Signal with more than 2 dimensions are not currently supported."; - return false; - } - - // Store the output signal - // TODO: hardcoded DataType::DOUBLE - pImpl->outputSignals.insert( - {portNumber, - std::make_shared(Signal::DataFormat::CONTIGUOUS_ZEROCOPY, DataType::DOUBLE)}); - - // Compute the width of the signal - unsigned numElements = 1; - for (auto dimension : dims) { - numElements *= dimension; - } - - // Configure the signal - pImpl->outputSignals[portNumber]->setWidth(numElements); - if (!pImpl->outputSignals[portNumber]->initializeBufferFromContiguousZeroCopy(address)) { - wbtError << "Failed to configure buffer for output signal connected to the port with index " - << portNumber << "."; - return false; - } - - // Store the dimensions in the map - pImpl->outputPortDimensions.emplace(portNumber, dims); - - return true; -} diff --git a/toolbox/core/src/ConvertStdVector.cpp b/toolbox/core/src/ConvertStdVector.cpp deleted file mode 100644 index 2a8da8ce2..000000000 --- a/toolbox/core/src/ConvertStdVector.cpp +++ /dev/null @@ -1,123 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/ConvertStdVector.h" -#include - -// Template definition -// =================== - -template -void wbt::convertStdVector(const std::vector& input, std::vector& output) -{ - output = std::vector(input.begin(), input.end()); -} - -// Explicit instantiation for all the other supported types -// ======================================================== - -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -// Bool to other numeric types -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -// Double to other numeric types -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); -// String to string -template void wbt::convertStdVector(const std::vector& input, - std::vector& output); - -// Explicit specialization for std::string type -// ============================================ - -template <> -void wbt::convertStdVector(const std::vector& input, - std::vector& output) -{ - output.clear(); - output.resize(input.size()); - std::transform(input.begin(), input.end(), output.begin(), [](const std::string& str) { - return std::stoi(str); - }); -} - -template <> -void wbt::convertStdVector(const std::vector& input, - std::vector& output) -{ - output.clear(); - output.resize(input.size()); - std::transform(input.begin(), input.end(), output.begin(), [](const std::string& str) { - return static_cast(std::stoi(str)); - }); -} - -template <> -void wbt::convertStdVector(const std::vector& input, - std::vector& output) -{ - output.clear(); - output.resize(input.size()); - std::transform(input.begin(), input.end(), output.begin(), [](const std::string& str) { - return std::stod(str); - }); -} - -template <> -void wbt::convertStdVector(const std::vector& input, - std::vector& output) -{ - output.clear(); - output.resize(input.size()); - std::transform(input.begin(), input.end(), output.begin(), [](const int& num) { - return std::to_string(num); - }); -} - -template <> -void wbt::convertStdVector(const std::vector& input, - std::vector& output) -{ - output.clear(); - output.resize(input.size()); - std::transform(input.begin(), input.end(), output.begin(), [](const bool& num) { - return std::to_string(num); - }); -} - -template <> -void wbt::convertStdVector(const std::vector& input, - std::vector& output) -{ - output.clear(); - output.resize(input.size()); - std::transform(input.begin(), input.end(), output.begin(), [](const double& num) { - return std::to_string(num); - }); -} - -// template <> -// void wbt::convertStdVector(const std::vector& input, -// std::vector& output) -//{ -// output = input; -// return; -//} diff --git a/toolbox/core/src/Log.cpp b/toolbox/core/src/Log.cpp deleted file mode 100644 index 8034366d8..000000000 --- a/toolbox/core/src/Log.cpp +++ /dev/null @@ -1,108 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/Log.h" - -#include - -using namespace wbt; - -class Log::impl -{ -public: - std::vector> errorsSStream; - std::vector> warningsSStream; - - const Verbosity verbosity = WBT_LOG_VERBOSITY; - - static std::string - serializeVectorStringStream(const std::vector>& ss); -}; - -Log::Log() - : pImpl{new Log::impl()} -{} - -Log& Log::getSingleton() -{ - static Log logInstance; - return logInstance; -} - -std::stringstream& Log::getLogStringStream(const Log::Type& type, - const std::string& file, - const unsigned& line, - const std::string& function) -{ - switch (pImpl->verbosity) { - case Log::Verbosity::RELEASE: - switch (type) { - case Log::Type::ERROR: - pImpl->errorsSStream.emplace_back(new std::stringstream); - return *pImpl->errorsSStream.back(); - case Log::Type::WARNING: - pImpl->warningsSStream.emplace_back(new std::stringstream); - return *pImpl->warningsSStream.back(); - } - case Log::Verbosity::DEBUG: - switch (type) { - case Log::Type::ERROR: { - pImpl->errorsSStream.emplace_back(new std::stringstream); - auto& ss = *pImpl->errorsSStream.back(); - ss << std::endl - << file << "@" << function << ":" << std::to_string(line) << std::endl; - return ss; - } - case Log::Type::WARNING: { - pImpl->warningsSStream.emplace_back(new std::stringstream); - auto& ss = *pImpl->warningsSStream.back(); - ss << std::endl - << file << "@" << function << ":" << std::to_string(line) << std::endl; - return ss; - } - } - } -} - -std::string -Log::impl::serializeVectorStringStream(const std::vector>& ss) -{ - std::stringstream output; - - for (const auto& ss_elem : ss) { - output << ss_elem->str() << std::endl; - } - - return output.str(); -} - -std::string Log::getErrors() const -{ - return impl::serializeVectorStringStream(pImpl->errorsSStream); -} - -std::string Log::getWarnings() const -{ - return impl::serializeVectorStringStream(pImpl->warningsSStream); -} - -void Log::clearWarnings() -{ - pImpl->warningsSStream.clear(); -} - -void Log::clearErrors() -{ - pImpl->errorsSStream.clear(); -} - -void Log::clear() -{ - clearErrors(); - clearWarnings(); -} diff --git a/toolbox/core/src/Parameter.cpp b/toolbox/core/src/Parameter.cpp deleted file mode 100644 index 0d23eb4cc..000000000 --- a/toolbox/core/src/Parameter.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/Parameter.h" - -using namespace wbt; - -const std::string defaultParamPrefix = "ParamIndex_"; - -ParameterMetadata::ParameterMetadata(const ParameterType& t, - const unsigned& paramIndex, - const int& paramRows, - const int& paramCols, - const std::string& paramName) - : index(paramIndex) - , name(paramName.empty() ? (defaultParamPrefix + std::to_string(index)) : paramName) - , rows(paramRows) - , cols(paramCols) - , type(t) -{} - -ParameterMetadata::ParameterMetadata(const ParameterMetadata& paramMD) - : index(paramMD.index) - , name(paramMD.name.empty() ? (defaultParamPrefix + std::to_string(index)) : paramMD.name) - , rows(paramMD.rows) - , cols(paramMD.cols) - , type(paramMD.type) -{} - -ParameterMetadata::ParameterMetadata(ParameterMetadata&& other) - : index(other.index) - , name(other.name.empty() ? (defaultParamPrefix + std::to_string(index)) : other.name) - , rows(other.rows) - , cols(other.cols) - , type(other.type) -{} - -ParameterMetadata& ParameterMetadata::operator=(const ParameterMetadata& other) -{ - *this = ParameterMetadata(other.type, other.index, other.rows, other.cols, other.name); - return *this; -} - -ParameterMetadata& ParameterMetadata::operator=(ParameterMetadata&& other) -{ - ParameterMetadata paramMD(other.type, other.index, other.rows, other.cols, other.name); - *this = paramMD; - return *this; -} - -bool ParameterMetadata::operator==(const ParameterMetadata& rhs) const -{ - bool ok = true; - ok = ok && (this->index == rhs.index); - ok = ok && (this->name == rhs.name); - ok = ok && (this->rows == rhs.rows); - ok = ok && (this->cols == rhs.cols); - ok = ok && (this->type == rhs.type); - return ok; -} diff --git a/toolbox/core/src/Parameters.cpp b/toolbox/core/src/Parameters.cpp deleted file mode 100644 index 95ae63703..000000000 --- a/toolbox/core/src/Parameters.cpp +++ /dev/null @@ -1,664 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/Parameters.h" -#include "Core/ConvertStdVector.h" -#include "Core/Log.h" -#include "Core/Parameter.h" - -#include -#include -#include -#include - -using namespace wbt; - -// ================ -// PARAMETERS::IMPL -// ================ - -class Parameters::impl -{ -public: - // Typedefs for generic scalar / vector parameters - using ParameterInt = Parameter; - using ParameterBool = Parameter; - using ParameterDouble = Parameter; - using ParameterString = Parameter; - - // Typedefs for the storage of vector parameters - using ParamVectorInt = std::vector; - using ParamVectorBool = std::vector; - using ParamVectorDouble = std::vector; - using ParamVectorString = std::vector; - - // Maps for storing parameters and their metadata - std::unordered_map paramsInt; - std::unordered_map paramsBool; - std::unordered_map paramsDouble; - std::unordered_map paramsString; - - // Maps for handling the internal indexing - std::unordered_map nameToType; - std::unordered_map indexToName; - std::unordered_map nameToIndex; - - bool existIndex(const ParamIndex& index) const; - bool existName(const ParamName& name, const wbt::ParameterType& type) const; - - impl* clone() { return new impl(*this); } -}; - -bool Parameters::impl::existName(const Parameters::ParamName& name, - const wbt::ParameterType& type) const -{ - switch (type) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: - if (paramsInt.find(name) == paramsInt.end()) { - return false; - } - break; - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: - if (paramsBool.find(name) == paramsBool.end()) { - return false; - } - break; - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: - if (paramsDouble.find(name) == paramsDouble.end()) { - return false; - } - break; - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: - if (paramsString.find(name) == paramsString.end()) { - return false; - } - break; - } - return true; -} - -bool Parameters::impl::existIndex(const Parameters::ParamIndex& index) const -{ - if (indexToName.find(index) == indexToName.end()) { - return false; - } - - return true; -} - -// ========== -// PARAMETERS -// ========== - -Parameters::Parameters() - : pImpl{new impl()} -{} - -// Defining the destructor as default here in the cpp avoids the usage -// of a custom pimpl deleter -Parameters::~Parameters() = default; - -Parameters::Parameters(const wbt::Parameters& other) - : pImpl{other.pImpl->clone()} -{} - -wbt::Parameters& Parameters::operator=(const wbt::Parameters& other) -{ - pImpl.reset(other.pImpl->clone()); - return *this; -} - -Parameters::ParamName Parameters::getParamName(const Parameters::ParamIndex& index) const -{ - if (pImpl->indexToName.find(index) == pImpl->indexToName.end()) { - return PARAM_INVALID_NAME; - } - - return pImpl->indexToName.at(index); -} - -Parameters::ParamIndex Parameters::getParamIndex(const Parameters::ParamName& name) const -{ - if (pImpl->nameToIndex.find(name) == pImpl->nameToIndex.end()) { - return PARAM_INVALID_INDEX; - } - - return pImpl->nameToIndex.at(name); -} - -bool Parameters::existName(const Parameters::ParamName& name) const -{ - if (pImpl->existName(name, ParameterType::INT) || pImpl->existName(name, ParameterType::BOOL) - || pImpl->existName(name, ParameterType::DOUBLE) - || pImpl->existName(name, ParameterType::STRING) - || pImpl->existName(name, ParameterType::STRUCT_INT) - || pImpl->existName(name, ParameterType::STRUCT_BOOL) - || pImpl->existName(name, ParameterType::STRUCT_DOUBLE) - || pImpl->existName(name, ParameterType::STRUCT_STRING)) { - return true; - } - return false; -} - -unsigned Parameters::getNumberOfParameters() const -{ - const size_t numIntParams = pImpl->paramsInt.size(); - const size_t numBoolParams = pImpl->paramsBool.size(); - const size_t numDoubleParams = pImpl->paramsDouble.size(); - const size_t numStringParams = pImpl->paramsString.size(); - - return numIntParams + numBoolParams + numDoubleParams + numStringParams; -} - -std::vector> Parameters::getIntParameters() const -{ - std::vector> vectorParams; - - for (auto p : pImpl->paramsInt) { - vectorParams.push_back(p.second); - } - - return vectorParams; -} - -std::vector> Parameters::getBoolParameters() const -{ - std::vector> vectorParams; - - for (auto p : pImpl->paramsBool) { - vectorParams.push_back(p.second); - } - - return vectorParams; -} - -std::vector> Parameters::getDoubleParameters() const -{ - std::vector> vectorParams; - - for (auto p : pImpl->paramsDouble) { - vectorParams.push_back(p.second); - } - - return vectorParams; -} - -std::vector> Parameters::getStringParameters() const -{ - std::vector> vectorParams; - - for (auto p : pImpl->paramsString) { - vectorParams.push_back(p.second); - } - - return vectorParams; -} - -wbt::ParameterMetadata Parameters::getParameterMetadata(const ParamName& name) -{ - if (!existName(name) || !pImpl->existName(name, pImpl->nameToType.at(name))) { - // TODO: here dummy metadata are returned. This can be improved. - wbtError << "Failed to get metadata of " << name << " parameter."; - return {ParameterType::INT, 0, 0, 0, "dummy"}; - } - - switch (pImpl->nameToType[name]) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: - return pImpl->paramsInt.at(name).getMetadata(); - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: - return pImpl->paramsBool.at(name).getMetadata(); - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: - return pImpl->paramsDouble.at(name).getMetadata(); - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: - return pImpl->paramsString.at(name).getMetadata(); - } -} - -// ========= -// TEMPLATES -// ========= - -// GETPARAMETER -// ============ - -// SCALAR -// ------ - -// Instantiate the declared templates -template bool Parameters::getParameter(const Parameters::ParamName& name, int& param) const; -template bool Parameters::getParameter(const Parameters::ParamName& name, bool& param) const; -template bool Parameters::getParameter(const Parameters::ParamName& name, - double& param) const; - -template -bool wbt::Parameters::getParameter(const wbt::Parameters::ParamName& name, T& param) const -{ - if (!existName(name) || !pImpl->existName(name, pImpl->nameToType.at(name))) { - wbtError << "Trying to get a non existing " << name << " parameter."; - return false; - } - - switch (pImpl->nameToType.at(name)) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: - if (!pImpl->paramsInt.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = static_cast(pImpl->paramsInt.at(name).getScalarParameter()); - break; - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: - if (!pImpl->paramsBool.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = static_cast(pImpl->paramsBool.at(name).getScalarParameter()); - break; - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: - if (!pImpl->paramsDouble.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = static_cast(pImpl->paramsDouble.at(name).getScalarParameter()); - break; - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: - if (!pImpl->paramsString.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = static_cast(std::stod(pImpl->paramsString.at(name).getScalarParameter())); - break; - } - return true; -} - -// VECTOR -// ------ - -template bool Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; -template bool Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; -template bool Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; -template bool Parameters::getParameter(const Parameters::ParamName& name, - std::vector& param) const; - -template -bool wbt::Parameters::getParameter(const wbt::Parameters::ParamName& name, - std::vector& param) const -{ - if (!existName(name) || !pImpl->existName(name, pImpl->nameToType.at(name))) { - wbtError << "Trying to get a non existing " << name << " parameter."; - return false; - } - - param.clear(); - - switch (pImpl->nameToType.at(name)) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: { - if (pImpl->paramsInt.at(name).isScalar()) { - wbtError << "Trying to get a vector from a scalar parameter."; - return false; - } - std::vector output; - convertStdVector(pImpl->paramsInt.at(name).getVectorParameter(), param); - break; - } - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: { - if (pImpl->paramsBool.at(name).isScalar()) { - wbtError << "Trying to get a vector from a scalar parameter."; - return false; - } - std::vector output; - convertStdVector(pImpl->paramsBool.at(name).getVectorParameter(), param); - break; - } - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: { - if (pImpl->paramsDouble.at(name).isScalar()) { - wbtError << "Trying to get a vector from a scalar parameter."; - return false; - } - std::vector output; - convertStdVector(pImpl->paramsDouble.at(name).getVectorParameter(), param); - break; - } - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: { - if (pImpl->paramsString.at(name).isScalar()) { - wbtError << "Trying to get a vector from a scalar parameter."; - return false; - } - std::vector output; - convertStdVector(pImpl->paramsString.at(name).getVectorParameter(), param); - break; - } - } - return true; -} - -// STOREPARAMETER -// ============ - -// SCALAR -// ------ - -template bool Parameters::storeParameter(const int& param, - const ParameterMetadata& paramMetadata); -template bool Parameters::storeParameter(const bool& param, - const ParameterMetadata& paramMetadata); -template bool Parameters::storeParameter(const double& param, - const ParameterMetadata& paramMetadata); - -template -bool wbt::Parameters::storeParameter(const T& param, const wbt::ParameterMetadata& paramMetadata) -{ - if (existName(paramMetadata.name) || pImpl->existName(paramMetadata.name, paramMetadata.type)) { - wbtError << "Trying to store an already existing " << paramMetadata.name << " parameter."; - return false; - } - - if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { - wbtError << "2D parameters are not supported."; - return false; - } - - switch (paramMetadata.type) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: - pImpl->paramsInt.emplace(std::make_pair( - paramMetadata.name, impl::ParameterInt(static_cast(param), paramMetadata))); - break; - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: - pImpl->paramsBool.emplace(std::make_pair( - paramMetadata.name, impl::ParameterBool(static_cast(param), paramMetadata))); - break; - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: - pImpl->paramsDouble.emplace( - std::make_pair(paramMetadata.name, - impl::ParameterDouble(static_cast(param), paramMetadata))); - break; - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: - pImpl->paramsString.emplace(std::make_pair( - paramMetadata.name, impl::ParameterString(std::to_string(param), paramMetadata))); - break; - } - - pImpl->nameToType[paramMetadata.name] = paramMetadata.type; - pImpl->nameToIndex[paramMetadata.name] = paramMetadata.index; - pImpl->indexToName[paramMetadata.index] = paramMetadata.name; - - return true; -} - -// VECTOR -// ------ - -template bool Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); -template bool Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); -template bool Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); -template bool Parameters::storeParameter(const std::vector& param, - const ParameterMetadata& paramMetadata); - -template -bool wbt::Parameters::storeParameter(const std::vector& param, - const wbt::ParameterMetadata& paramMetadata) -{ - if (existName(paramMetadata.name) || pImpl->existName(paramMetadata.name, paramMetadata.type)) { - wbtError << "Trying to store an already existing " << paramMetadata.name << " parameter."; - return false; - } - - if (paramMetadata.rows != 1 && paramMetadata.cols != param.size()) { - wbtError << "2D parameters are not supported."; - return false; - } - - switch (paramMetadata.type) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: { - std::vector paramInt(param.size()); - convertStdVector(param, paramInt); - pImpl->paramsInt.emplace( - std::make_pair(paramMetadata.name, impl::ParameterInt(paramInt, paramMetadata))); - break; - } - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: { - std::vector paramBool(param.size()); - convertStdVector(param, paramBool); - pImpl->paramsBool.emplace( - std::make_pair(paramMetadata.name, impl::ParameterBool(paramBool, paramMetadata))); - break; - } - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: { - std::vector paramDouble(param.size()); - convertStdVector(param, paramDouble); - pImpl->paramsDouble.emplace(std::make_pair( - paramMetadata.name, impl::ParameterDouble(paramDouble, paramMetadata))); - break; - } - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: { - std::vector paramString(param.size()); - convertStdVector(param, paramString); - pImpl->paramsString.emplace(std::make_pair( - paramMetadata.name, impl::ParameterString(paramString, paramMetadata))); - break; - } - } - - pImpl->nameToType[paramMetadata.name] = paramMetadata.type; - pImpl->nameToIndex[paramMetadata.name] = paramMetadata.index; - pImpl->indexToName[paramMetadata.index] = paramMetadata.name; - - return true; -} - -// PARAMETER -// --------- - -template bool Parameters::storeParameter(const Parameter& parameter); -template bool Parameters::storeParameter(const Parameter& parameter); -template bool Parameters::storeParameter(const Parameter& parameter); -template bool Parameters::storeParameter(const Parameter& parameter); - -template -bool wbt::Parameters::storeParameter(const wbt::Parameter& parameter) -{ - if (existName(parameter.getMetadata().name)) { - wbtError << "Trying to store an already existing " << parameter.getMetadata().name - << " parameter."; - return false; - } - - if (parameter.isScalar()) { - return storeParameter(parameter.getScalarParameter(), parameter.getMetadata()); - } - else { - return storeParameter(parameter.getVectorParameter(), parameter.getMetadata()); - } -} - -// TEMPLATE SPECIALIZATIONS -// ======================== - -template <> -bool Parameters::getParameter(const ParamName& name, std::string& param) const -{ - if (!existName(name) || !pImpl->existName(name, pImpl->nameToType.at(name))) { - wbtError << "Trying to get a non existing " << name << " parameter."; - return false; - } - - switch (pImpl->nameToType.at(name)) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: - if (!pImpl->paramsInt.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = std::to_string(pImpl->paramsInt.at(name).getScalarParameter()); - break; - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: - if (!pImpl->paramsBool.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = std::to_string(pImpl->paramsBool.at(name).getScalarParameter()); - break; - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: - if (!pImpl->paramsDouble.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = std::to_string(pImpl->paramsDouble.at(name).getScalarParameter()); - break; - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: - if (!pImpl->paramsString.at(name).isScalar()) { - wbtError << "Trying to get a scalar from a vector parameter."; - return false; - } - param = pImpl->paramsString.at(name).getScalarParameter(); - break; - } - return true; -} - -template <> -bool wbt::Parameters::storeParameter(const std::string& param, - const wbt::ParameterMetadata& paramMetadata) -{ - if (existName(paramMetadata.name) || pImpl->existName(paramMetadata.name, paramMetadata.type)) { - wbtError << "Trying to store an already existing " << paramMetadata.name << " parameter."; - return false; - } - - if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { - wbtError << "2D parameters are not supported."; - return false; - } - - switch (paramMetadata.type) { - case ParameterType::INT: - case ParameterType::CELL_INT: - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_CELL_INT: - pImpl->paramsInt.emplace(std::make_pair( - paramMetadata.name, impl::ParameterInt(std::stoi(param), paramMetadata))); - break; - case ParameterType::BOOL: - case ParameterType::CELL_BOOL: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_CELL_BOOL: - pImpl->paramsBool.emplace(std::make_pair( - paramMetadata.name, - impl::ParameterBool(static_cast(std::stoi(param)), paramMetadata))); - break; - case ParameterType::DOUBLE: - case ParameterType::CELL_DOUBLE: - case ParameterType::STRUCT_DOUBLE: - case ParameterType::STRUCT_CELL_DOUBLE: - pImpl->paramsDouble.emplace(std::make_pair( - paramMetadata.name, impl::ParameterDouble(std::stod(param), paramMetadata))); - break; - case ParameterType::STRING: - case ParameterType::CELL_STRING: - case ParameterType::STRUCT_STRING: - case ParameterType::STRUCT_CELL_STRING: - pImpl->paramsString.emplace( - std::make_pair(paramMetadata.name, impl::ParameterString(param, paramMetadata))); - break; - } - - pImpl->nameToType[paramMetadata.name] = paramMetadata.type; - pImpl->nameToIndex[paramMetadata.name] = paramMetadata.index; - pImpl->indexToName[paramMetadata.index] = paramMetadata.name; - - return true; -} diff --git a/toolbox/core/src/Signal.cpp b/toolbox/core/src/Signal.cpp deleted file mode 100644 index 2b053d820..000000000 --- a/toolbox/core/src/Signal.cpp +++ /dev/null @@ -1,373 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/Signal.h" -#include "Core/Log.h" - -#include -#include -#include -#include -#include -#include - -using namespace wbt; - -// ============ -// SIGNAL::IMPL -// ============ - -class Signal::impl -{ -public: - int width = Signal::DynamicSize; - const DataType portDataType; - const DataFormat dataFormat; - - void* bufferPtr = nullptr; - - template - T* getBufferImpl(); - - void deleteBuffer(); - void allocateBuffer(const void* const bufferInput, void*& bufferOutput, const unsigned& length); - - impl(const DataFormat& dFormat, const DataType& dType) - : portDataType(dType) - , dataFormat(dFormat) - {} - - impl* clone() { return new impl(*this); } -}; - -void Signal::impl::allocateBuffer(const void* const bufferInput, - void*& bufferOutput, - const unsigned& length) -{ - if (dataFormat == DataFormat::CONTIGUOUS_ZEROCOPY) { - wbtWarning << "Trying to allocate a buffer with a non-supported " - << "CONTIGUOUS_ZEROCOPY data format."; - return; - } - - switch (portDataType) { - case DataType::DOUBLE: { - // Allocate the array - bufferOutput = static_cast(new double[length]); - // 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: - // TODO: Implement other DataType - wbtError << "The specified DataType is not yet supported. Used DOUBLE instead."; - return; - } -} - -void Signal::impl::deleteBuffer() -{ - if (dataFormat == DataFormat::CONTIGUOUS_ZEROCOPY || !bufferPtr) { - return; - } - - switch (portDataType) { - case DataType::DOUBLE: - delete static_cast(bufferPtr); - bufferPtr = nullptr; - return; - default: - // TODO: Implement other DataType - wbtError << "The specified DataType is not yet supported. Used DOUBLE instead."; - return; - } -} - -// ====== -// SIGNAL -// ====== - -Signal::~Signal() -{ - pImpl->deleteBuffer(); -} - -Signal::Signal(const Signal& other) - : pImpl{other.pImpl->clone()} -{ - if (pImpl->bufferPtr) { - switch (pImpl->dataFormat) { - case DataFormat::CONTIGUOUS_ZEROCOPY: - // We just need the buffer pointer, which has been already copied - // by the pImpl clone. - break; - case DataFormat::NONCONTIGUOUS: - case DataFormat::CONTIGUOUS: - // Copy the allocated data - pImpl->allocateBuffer(other.pImpl->bufferPtr, pImpl->bufferPtr, other.pImpl->width); - break; - } - } -} - -Signal::Signal(const DataFormat& dataFormat, const DataType& dataType) - : pImpl{new impl(dataFormat, dataType)} -{} - -Signal::Signal(Signal&& other) - : pImpl{other.pImpl->clone()} -{ - other.pImpl->width = 0; - other.pImpl->bufferPtr = nullptr; -} - -bool Signal::initializeBufferFromContiguousZeroCopy(const void* buffer) -{ - if (pImpl->dataFormat != DataFormat::CONTIGUOUS_ZEROCOPY) { - wbtError << "Trying to initialize a CONTIGUOUS_ZEROCOPY signal but the configured " - << "DataFormat does not match."; - return false; - } - - pImpl->bufferPtr = const_cast(buffer); - return true; -} - -bool Signal::initializeBufferFromContiguous(const void* buffer) -{ - if (pImpl->dataFormat != DataFormat::CONTIGUOUS) { - wbtError << "Trying to initialize a CONTIGUOUS signal but the configured " - << "DataFormat does not match."; - return false; - } - - if (pImpl->width <= 0) { - wbtError << "Signal width unknown. Unable to initialize the buffer if the " - << "signal size is not set."; - return false; - } - - // Copy data from the external contiguous buffer to the internal buffer - pImpl->allocateBuffer(buffer, pImpl->bufferPtr, pImpl->width); - - return true; -} - -bool Signal::initializeBufferFromNonContiguous(const void* const* bufferPtrs) -{ - if (pImpl->dataFormat != DataFormat::NONCONTIGUOUS) { - wbtError << "Trying to initialize a NONCONTIGUOUS signal but the configured " - << "DataFormat does not match."; - return false; - } - - if (pImpl->width <= 0) { - wbtError << "Signal width unknown. Unable to initialize the buffer if the " - << "signal size is not set."; - return false; - } - - if (pImpl->portDataType == DataType::DOUBLE) { - // Allocate a new vector to store data from the non-contiguous signal - pImpl->bufferPtr = static_cast(new double[pImpl->width]); - double* bufferPtrDouble = static_cast(pImpl->bufferPtr); - - // Copy data from MATLAB's memory to the Signal object - for (auto i = 0; i < pImpl->width; ++i) { - const double* valuePtr = static_cast(*bufferPtrs); - bufferPtrDouble[i] = valuePtr[i]; - } - } - return true; -} - -bool Signal::isValid() const -{ - return pImpl->bufferPtr && (pImpl->width > 0); -} - -void Signal::setWidth(const unsigned width) -{ - pImpl->width = width; -} - -int Signal::getWidth() const -{ - return pImpl->width; -} - -DataType Signal::getPortDataType() const -{ - return pImpl->portDataType; -} - -Signal::DataFormat Signal::getDataFormat() const -{ - return pImpl->dataFormat; -} - -bool Signal::set(const unsigned index, const double data) -{ - if (pImpl->width <= index) { - wbtError << "The signal index exceeds its width."; - return false; - } - - if (!pImpl->bufferPtr) { - wbtError << "The pointer to data is null. The signal was not configured properly."; - return false; - } - - switch (pImpl->portDataType) { - case DataType::DOUBLE: { - double* buffer = static_cast(pImpl->bufferPtr); - buffer[index] = data; - break; - } - case DataType::SINGLE: { - float* buffer = static_cast(pImpl->bufferPtr); - buffer[index] = data; - break; - } - default: - // TODO: Implement other DataType - wbtError << "The specified DataType is not yet supported. Used DOUBLE instead."; - return false; - break; - } - return true; -} - -// Explicit template instantiations -// ================================ -template double* Signal::getBuffer(); -template const double* Signal::getBuffer() const; -template double Signal::get(const unsigned i) const; -template bool Signal::setBuffer(const double* data, const unsigned length); - -// Template definitions -// =================== - -template -T Signal::get(const unsigned i) const -{ - const T* buffer = getBuffer(); - - if (!buffer) { - wbtError << "The buffer inside the signal has not been initialized properly."; - return {}; - } - - if (i >= pImpl->width) { - wbtError << "Trying to access an element that exceeds signal width."; - return {}; - } - - return buffer[i]; -} - -template -T* Signal::impl::getBufferImpl() -{ - const std::map mapDataTypeToHash = { - {DataType::DOUBLE, typeid(double).hash_code()}, - {DataType::SINGLE, typeid(float).hash_code()}, - {DataType::INT8, typeid(int8_t).hash_code()}, - {DataType::UINT8, typeid(uint8_t).hash_code()}, - {DataType::INT16, typeid(int16_t).hash_code()}, - {DataType::UINT16, typeid(uint16_t).hash_code()}, - {DataType::INT32, typeid(int32_t).hash_code()}, - {DataType::UINT32, typeid(uint32_t).hash_code()}, - {DataType::BOOLEAN, typeid(bool).hash_code()}}; - - if (!bufferPtr) { - wbtError << "The pointer to data is null. The signal was not configured properly."; - return nullptr; - } - - // Check the returned matches the same type of the portType. - // If this is not met, applying pointer arithmetics on the returned - // pointer would show unknown behaviour. - if (typeid(T).hash_code() != mapDataTypeToHash.at(portDataType)) { - wbtError << "Trying to get the buffer using a type different than its DataType"; - return nullptr; - } - - // Cast pointer and return it - return static_cast(bufferPtr); -} - -template -T* Signal::getBuffer() -{ - return pImpl->getBufferImpl(); -} - -template -const T* Signal::getBuffer() const -{ - return pImpl->getBufferImpl(); -} - -template -bool Signal::setBuffer(const T* data, const unsigned length) -{ - // Non contiguous signals follow the Simulink convention of being read-only. - // They are used only for input signals. - if (pImpl->dataFormat == DataFormat::NONCONTIGUOUS) { - wbtError << "Changing buffer address to NONCONTIGUOUS is not allowed."; - return false; - } - - // Fail if the length is greater of the signal width - if (pImpl->dataFormat == DataFormat::CONTIGUOUS_ZEROCOPY && length > pImpl->width) { - wbtError << "Trying to set a buffer with a length greater than the signal width."; - return false; - } - - // Check that T matches the type of raw buffer stored. Use getBuffer since it will return - // nullptr if this is not met. - if (!getBuffer()) { - wbtError << "Trying to get a pointer with a type not matching the signal's DataType."; - return false; - } - - switch (pImpl->dataFormat) { - case DataFormat::CONTIGUOUS: - // Delete the current array - if (pImpl->bufferPtr) { - delete getBuffer(); - pImpl->bufferPtr = nullptr; - pImpl->width = 0; - } - // Allocate a new empty array - pImpl->bufferPtr = static_cast(new T[length]); - pImpl->width = length; - // Fill it with new data - std::copy(data, data + length, getBuffer()); - break; - case DataFormat::CONTIGUOUS_ZEROCOPY: - // Reset current data if width changes - if (length != pImpl->width) { - std::fill(getBuffer(), getBuffer() + length, 0); - } - // Copy new data - std::copy(data, data + length, getBuffer()); - // Update the width - pImpl->width = length; - break; - case DataFormat::NONCONTIGUOUS: - wbtError << "The code should never arrive here. Unexpected error."; - return false; - } - - return true; -} diff --git a/toolbox/core/src/SimulinkBlockInformation.cpp b/toolbox/core/src/SimulinkBlockInformation.cpp deleted file mode 100644 index 0c3348f9c..000000000 --- a/toolbox/core/src/SimulinkBlockInformation.cpp +++ /dev/null @@ -1,900 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#include "Core/SimulinkBlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" -#include "MxAnyType.h" - -#include -#include -#include -#include -#include -#include - -using namespace wbt; - -SimulinkBlockInformation::SimulinkBlockInformation(SimStruct* S) - : simstruct(S) -{} - -// BLOCK OPTIONS METHODS -// ===================== - -bool SimulinkBlockInformation::optionFromKey(const std::string& key, double& option) const -{ - if (key == wbt::BlockOptionPrioritizeOrder) { - option = SS_OPTION_PLACE_ASAP; - return true; - } - - wbtError << "Unrecognized block option."; - return false; -} - -// PARAMETERS METHODS -// ================== - -bool SimulinkBlockInformation::getStringParameterAtIndex(const ParameterIndex idx, - std::string& value) const -{ - const mxArray* blockParam = ssGetSFcnParam(simstruct, idx); - return MxAnyType(blockParam).asString(value); -} - -bool SimulinkBlockInformation::getScalarParameterAtIndex(const ParameterIndex idx, - double& value) const -{ - const mxArray* blockParam = ssGetSFcnParam(simstruct, idx); - return MxAnyType(blockParam).asDouble(value); -} - -bool SimulinkBlockInformation::getBooleanParameterAtIndex(const ParameterIndex idx, - bool& value) const -{ - double tmpValue = 0; - const mxArray* blockParam = ssGetSFcnParam(simstruct, idx); - - // 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; - } - else if (MxAnyType(blockParam).asDouble(tmpValue)) { - value = static_cast(tmpValue); - return true; - } - - wbtError << "Failed to parse bool parameter"; - return false; -} - -bool SimulinkBlockInformation::getCellAtIndex(const ParameterIndex idx, AnyCell& value) const -{ - const mxArray* blockParam = ssGetSFcnParam(simstruct, idx); - return MxAnyType(blockParam).asAnyCell(value); -} - -bool SimulinkBlockInformation::getStructAtIndex(const ParameterIndex idx, AnyStruct& value) const -{ - const mxArray* blockParam = ssGetSFcnParam(simstruct, idx); - return MxAnyType(blockParam).asAnyStruct(value); -} - -bool SimulinkBlockInformation::getVectorAtIndex(const ParameterIndex idx, - std::vector& value) const -{ - const mxArray* blockParam = ssGetSFcnParam(simstruct, idx); - return MxAnyType(blockParam).asVectorDouble(value); -} - -bool SimulinkBlockInformation::getStringFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - std::string& value) const -{ - AnyStruct s; - - if (!getStructAtIndex(idx, s)) { - wbtError << "Failed to get struct at index " << idx << "."; - return false; - } - - if (s.find(fieldName) == s.end()) { - wbtError << "Struct at index " << idx << " does not contain any " << fieldName << " field."; - return false; - } - - return s.at(fieldName)->asString(value); -} - -bool SimulinkBlockInformation::getScalarFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - double& value) const -{ - AnyStruct s; - - if (!getStructAtIndex(idx, s)) { - wbtError << "Failed to get struct at index " << idx << "."; - return false; - } - - if (s.find(fieldName) == s.end()) { - wbtError << "Struct at index " << idx << " does not contain any " << fieldName << " field."; - return false; - } - - return s.at(fieldName)->asDouble(value); -} - -bool SimulinkBlockInformation::getBooleanFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - bool& value) const -{ - AnyStruct s; - - if (!getStructAtIndex(idx, s)) { - wbtError << "Failed to get struct at index " << idx << "."; - return false; - } - - if (s.find(fieldName) == s.end()) { - wbtError << "Struct at index " << idx << " does not contain any " << fieldName << " field."; - return false; - } - - return s.at(fieldName)->asBool(value); -} - -bool SimulinkBlockInformation::getCellFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - AnyCell& value) const -{ - AnyStruct s; - - if (!getStructAtIndex(idx, s)) { - wbtError << "Failed to get struct at index " << idx << "."; - return false; - } - - if (s.find(fieldName) == s.end()) { - wbtError << "Struct at index " << idx << " does not contain any " << fieldName << " field."; - return false; - } - - return s.at(fieldName)->asAnyCell(value); -} - -bool SimulinkBlockInformation::getVectorDoubleFieldAtIndex(const ParameterIndex idx, - const std::string& fieldName, - std::vector& value) const -{ - AnyStruct s; - - if (!getStructAtIndex(idx, s)) { - wbtError << "Failed to get struct at index " << idx << "."; - return false; - } - - if (s.find(fieldName) == s.end()) { - wbtError << "Struct at index " << idx << " does not contain any " << fieldName << " field."; - return false; - } - - return s.at(fieldName)->asVectorDouble(value); -} - -// PORT INFORMATION SETTERS -// ======================== - -bool SimulinkBlockInformation::setNumberOfInputPorts(const unsigned numberOfPorts) -{ - return ssSetNumInputPorts(simstruct, numberOfPorts); -} - -bool SimulinkBlockInformation::setNumberOfOutputPorts(const unsigned numberOfPorts) -{ - return ssSetNumOutputPorts(simstruct, numberOfPorts); -} - -bool SimulinkBlockInformation::setInputPortVectorSize(const PortIndex idx, const VectorSize& size) -{ - if (size == Signal::DynamicSize) { - // TODO: in this case, explore how to use mdlSetOutputPortDimensionInfo and - // mdlSetDefaultPortDimensionInfo - return ssSetInputPortVectorDimension(simstruct, idx, DYNAMICALLY_SIZED); - } - - return ssSetInputPortVectorDimension(simstruct, idx, size); -} - -bool SimulinkBlockInformation::setInputPortMatrixSize(const PortIndex idx, const MatrixSize& size) -{ - // Refer to: https://it.mathworks.com/help/simulink/sfg/sssetoutputportmatrixdimensions.html - if (size.first == Signal::DynamicSize || size.second == Signal::DynamicSize) { - // TODO: in this case, explore how to use mdlSetOutputPortDimensionInfo and - // mdlSetDefaultPortDimensionInfo - ssSetInputPortMatrixDimensions(simstruct, idx, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED); - } - - return ssSetInputPortMatrixDimensions(simstruct, idx, size.first, size.first); -} - -bool SimulinkBlockInformation::setOutputPortVectorSize(const PortIndex idx, const VectorSize& size) -{ - if (size == Signal::DynamicSize) { - // TODO: in this case, explore how to use mdlSetOutputPortDimensionInfo and - // mdlSetDefaultPortDimensionInfo - return ssSetOutputPortVectorDimension(simstruct, idx, DYNAMICALLY_SIZED); - } - - return ssSetOutputPortVectorDimension(simstruct, idx, size); -} - -bool SimulinkBlockInformation::setOutputPortMatrixSize(const PortIndex idx, const MatrixSize& size) -{ - // Refer to: https://it.mathworks.com/help/simulink/sfg/sssetinputportmatrixdimensions.html - if (size.first == Signal::DynamicSize || size.second == Signal::DynamicSize) { - // TODO: in this case, explore how to use mdlSetOutputPortDimensionInfo and - // mdlSetDefaultPortDimensionInfo - return ssSetOutputPortMatrixDimensions( - simstruct, idx, DYNAMICALLY_SIZED, DYNAMICALLY_SIZED); - } - - return ssSetOutputPortMatrixDimensions(simstruct, idx, size.first, size.second); -} - -bool SimulinkBlockInformation::setInputPortType(const PortIndex idx, const wbt::DataType type) -{ - ssSetInputPortDirectFeedThrough(simstruct, idx, 1); - ssSetInputPortDataType(simstruct, idx, mapPortTypeToSimulink(type)); - return true; -} - -bool SimulinkBlockInformation::setOutputPortType(const PortIndex idx, const wbt::DataType type) -{ - ssSetOutputPortDataType(simstruct, idx, mapPortTypeToSimulink(type)); - return true; -} - -// PORT INFORMATION GETTERS -// ======================== - -BlockInformation::VectorSize SimulinkBlockInformation::getInputPortWidth(const PortIndex idx) const -{ - return ssGetInputPortWidth(simstruct, idx); -} - -BlockInformation::VectorSize SimulinkBlockInformation::getOutputPortWidth(const PortIndex idx) const -{ - return ssGetOutputPortWidth(simstruct, idx); -} - -wbt::InputSignalPtr SimulinkBlockInformation::getInputPortSignal(const PortIndex idx, - const VectorSize size) const -{ - // Read if the signal is contiguous or non-contiguous - boolean_T isContiguous = ssGetInputPortRequiredContiguous(simstruct, idx); - Signal::DataFormat sigDataFormat = - isContiguous ? Signal::DataFormat::CONTIGUOUS_ZEROCOPY : Signal::DataFormat::NONCONTIGUOUS; - - // Check if the signal is dynamically sized (which means that the dimension - // cannot be read) - bool isDynamicallySized = (ssGetInputPortWidth(simstruct, idx) == DYNAMICALLY_SIZED); - - // Note that if the signal is dynamically sized, portWidth is necessary - if (isDynamicallySized && size == Signal::DynamicSize) { - wbtError << "Trying to get a dynamically sized signal without specifying its size."; - return {}; - } - - // Read the width of the signal if it is not provided as input and the signal is not - // dynamically sized - VectorSize signalSize = size; - if (!isDynamicallySized && size == Signal::DynamicSize) { - signalSize = ssGetInputPortWidth(simstruct, idx); - } - - // Get the data type of the Signal if set (default: double) - DTypeId dataType = ssGetInputPortDataType(simstruct, idx); - - switch (sigDataFormat) { - case Signal::DataFormat::CONTIGUOUS_ZEROCOPY: { - // Initialize the signal - auto signal = std::make_shared(Signal::DataFormat::CONTIGUOUS_ZEROCOPY, - mapSimulinkToPortType(dataType)); - signal->setWidth(signalSize); - - // Initialize signal's data - if (!signal->initializeBufferFromContiguousZeroCopy( - ssGetInputPortSignal(simstruct, idx))) { - wbtError << "Failed to inititialize CONTIGUOUS_ZEROCOPY signal at index " << idx - << "."; - return {}; - } - - // Check signal validity - if (!signal->isValid()) { - wbtError << "Input signal at index " << idx << " is not valid."; - return {}; - } - - return signal; - } - case Signal::DataFormat::NONCONTIGUOUS: { - // Initialize the signal - auto signal = std::make_shared(Signal::DataFormat::NONCONTIGUOUS, - mapSimulinkToPortType(dataType)); - signal->setWidth(signalSize); - - // Initialize signal's data - InputPtrsType port = ssGetInputPortSignalPtrs(simstruct, idx); - if (!signal->initializeBufferFromNonContiguous(static_cast(port))) { - wbtError << "Failed to inititialize NONCONTIGUOUS signal at index " << idx << "."; - return {}; - } - - // Check signal validity - if (!signal->isValid()) { - wbtError << "Input signal at index " << idx << " is not valid."; - return {}; - } - - return signal; - } - case Signal::DataFormat::CONTIGUOUS: { - wbtError << "Failed to inititialize CONTIGUOUS signal at index " << idx << "." - << std::endl - << "CONTIGUOUS input signals are not yet supported. " - << "Use CONTIGUOUS_ZEROCOPY instead."; - return {}; - } - } -} - -wbt::OutputSignalPtr SimulinkBlockInformation::getOutputPortSignal(const PortIndex idx, - const VectorSize size) const -{ - // Check if the signal is dynamically sized (which means that the dimension - // cannot be read) - bool isDynamicallySized = (ssGetOutputPortWidth(simstruct, idx) == DYNAMICALLY_SIZED); - - // Note that if the signal is dynamically sized, portWidth is necessary - if (isDynamicallySized && size == Signal::DynamicSize) { - wbtError << "Trying to get a dynamically sized signal without specifying its size."; - return {}; - } - - // Read the width of the signal if it is not provided as input and the signal is not - // dynamically sized - VectorSize signalSize = size; - if (!isDynamicallySized && size == Signal::DynamicSize) { - signalSize = ssGetOutputPortWidth(simstruct, idx); - } - - // Get the data type of the Signal if set (default: double) - DTypeId dataType = ssGetOutputPortDataType(simstruct, idx); - - auto signal = std::make_shared(Signal::DataFormat::CONTIGUOUS_ZEROCOPY, - mapSimulinkToPortType(dataType)); - signal->setWidth(signalSize); - - if (!signal->initializeBufferFromContiguousZeroCopy(ssGetOutputPortSignal(simstruct, idx))) { - wbtError << "Failed to inititialize CONTIGUOUS_ZEROCOPY signal at index " << idx << "."; - return {}; - } - - if (!signal->isValid()) { - wbtError << "Output signal at index " << idx << " is not valid."; - return {}; - } - - return signal; -} - -BlockInformation::MatrixSize -SimulinkBlockInformation::getInputPortMatrixSize(const PortIndex idx) const -{ - if (ssGetInputPortNumDimensions(simstruct, idx) < 2) { - wbtError << "Signal at index " << idx - << "does not contain a matrix. Failed to gete its size."; - return {}; - } - - const int_T* sizes = ssGetInputPortDimensions(simstruct, idx); - return {sizes[0], sizes[1]}; -} - -BlockInformation::MatrixSize -SimulinkBlockInformation::getOutputPortMatrixSize(const PortIndex idx) const -{ - if (ssGetOutputPortNumDimensions(simstruct, idx) < 2) { - wbtError << "Signal at index " << idx - << "does not contain a matrix. Failed to gete its size."; - return {}; - } - - const int_T* sizes = ssGetOutputPortDimensions(simstruct, idx); - return {sizes[0], sizes[1]}; -} - -DataType SimulinkBlockInformation::mapSimulinkToPortType(const DTypeId typeId) const -{ - switch (typeId) { - case SS_DOUBLE: - return DataType::DOUBLE; - case SS_SINGLE: - return DataType::SINGLE; - case SS_INT8: - return DataType::INT8; - case SS_UINT8: - return DataType::UINT8; - case SS_INT16: - return DataType::INT16; - case SS_UINT16: - return DataType::UINT16; - case SS_INT32: - return DataType::INT32; - case SS_UINT32: - return DataType::UINT32; - case SS_BOOLEAN: - return DataType::BOOLEAN; - default: - return DataType::DOUBLE; - } -} - -DTypeId SimulinkBlockInformation::mapPortTypeToSimulink(const wbt::DataType dataType) const -{ - switch (dataType) { - case DataType::DOUBLE: - return SS_DOUBLE; - case DataType::SINGLE: - return SS_SINGLE; - case DataType::INT8: - return SS_INT8; - case DataType::UINT8: - return SS_UINT8; - case DataType::INT16: - return SS_INT16; - case DataType::UINT16: - return SS_UINT16; - case DataType::INT32: - return SS_INT32; - case DataType::UINT32: - return SS_UINT32; - case DataType::BOOLEAN: - return SS_BOOLEAN; - } -} - -bool SimulinkBlockInformation::addParameterMetadata(const wbt::ParameterMetadata& paramMD) -{ - for (auto md : m_paramsMetadata) { - if (md.name == paramMD.name) { - wbtError << "Trying to store an already existing " << md.name << " parameter."; - return false; - } - } - - // Add the new metadata to the block information - m_paramsMetadata.push_back(paramMD); - return true; -} - -bool SimulinkBlockInformation::parseParameters(wbt::Parameters& parameters) -{ - auto metadataContainsScalarParam = [](const wbt::ParameterMetadata& md) -> const bool { - return md.rows == 1 && md.cols == 1; - }; - - for (wbt::ParameterMetadata paramMD : m_paramsMetadata) { - - bool ok; - - // TODO Right now the cells are reshaped to a 1 x NumElements by MxAnyType - if (paramMD.rows == ParameterMetadata::DynamicSize) { - wbtError << "Dynamically sized rows are not currently supported."; - return false; - } - - // Handle the case of dynamically sized columns. In this case the metadata passed - // from the Block (containing DynamicSize) is modified with the length of the - // vector that is going to be stored. - // This is necessary in the pipeline for storing the metadata in the RTW file, which should - // not have any dynamic size. - const bool hasDynSizeColumns = (paramMD.cols == ParameterMetadata::DynamicSize); - auto handleDynSizeColumns = [](int& sizeToUpdate, const int& realSize) -> const bool { - if (realSize == ParameterMetadata::DynamicSize) { - wbtError << "Trying to store the cols of a dynamically sized parameters, but the " - << "metadata does not specify a valid size. Probably the block didn't " - << "updat the size in its initialization phase."; - return false; - } - sizeToUpdate = realSize; - return true; - }; - - switch (paramMD.type) { - // SCALAR / VECTOR PARAMETERS - // -------------------------- - // - // getScalarParameterAtIndex and getVectorAtIndex operate on type double. - // The cast to other types is handled by storeParameter internally, - // accordingly to the type stored in the metadata. - // - // Despite bool has its own bool parser, considering that both int and double - // are loaded as double (Simulink limitation), in order to simplify the - // maintainability of this code, everything is handled as double. - // - case ParameterType::INT: - case ParameterType::BOOL: - case ParameterType::DOUBLE: { - if (metadataContainsScalarParam(paramMD)) { - double paramValue; - if (!getScalarParameterAtIndex(paramMD.index, paramValue)) { - wbtError << "Failed to get scalar parameter at index " << paramMD.index - << "."; - return false; - } - ok = parameters.storeParameter(paramValue, paramMD); - } - else { - std::vector paramVector; - if (!getVectorAtIndex(paramMD.index, paramVector)) { - wbtError << "Failed to get vector parameter at index " << paramMD.index - << "."; - return false; - } - if (hasDynSizeColumns) { - if (!handleDynSizeColumns(paramMD.cols, paramVector.size())) { - return false; - } - } - ok = parameters.storeParameter(paramVector, paramMD); - } - break; - } - case ParameterType::STRING: { - if (metadataContainsScalarParam(paramMD)) { - std::string paramValue; - if (!getStringParameterAtIndex(paramMD.index, paramValue)) { - wbtError << "Failed to get string parameter at index " << paramMD.index - << "."; - return false; - } - ok = parameters.storeParameter(paramValue, paramMD); - } - else { - wbtError << "Char arrays are not yet supported."; - return false; - } - break; - } - // CELL PARAMETERS - // --------------- - case ParameterType::CELL_INT: - case ParameterType::CELL_BOOL: - case ParameterType::CELL_DOUBLE: { - AnyCell cell; - if (!getCellAtIndex(paramMD.index, cell)) { - wbtError << "Failed to get cell parameter at index " << paramMD.index << "."; - return false; - } - std::vector paramVector; - for (auto element : cell) { - double value; - if (!element->asDouble(value)) { - wbtError << "Failed to parse an element of the cell at index " - << paramMD.index << " as a double."; - return false; - } - paramVector.push_back(value); - } - if (hasDynSizeColumns) { - if (!handleDynSizeColumns(paramMD.cols, paramVector.size())) { - return false; - } - } - ok = parameters.storeParameter(paramVector, paramMD); - break; - } - case ParameterType::CELL_STRING: { - AnyCell cell; - if (!getCellAtIndex(paramMD.index, cell)) { - wbtError << "Failed to get cell parameter at index " << paramMD.index << "."; - return false; - } - std::vector paramVector; - for (auto element : cell) { - std::string value; - if (!element->asString(value)) { - wbtError << "Failed to parse an element of the cell at index " - << paramMD.index << " as a string."; - return false; - } - paramVector.push_back(value); - } - if (hasDynSizeColumns) { - if (!handleDynSizeColumns(paramMD.cols, paramVector.size())) { - return false; - } - } - ok = parameters.storeParameter(paramVector, paramMD); - break; - } - // STRUCT PARAMETERS - // ----------------- - case ParameterType::STRUCT_INT: - case ParameterType::STRUCT_BOOL: - case ParameterType::STRUCT_DOUBLE: { - if (metadataContainsScalarParam(paramMD)) { - double paramValue; - if (!getScalarFieldAtIndex(paramMD.index, paramMD.name, paramValue)) { - wbtError << "Failed to get scalar field " << paramMD.name - << " from the struct at index " << paramMD.index << "."; - return false; - } - ok = parameters.storeParameter(paramValue, paramMD); - } - else { - std::vector paramVector; - if (!getVectorDoubleFieldAtIndex(paramMD.index, paramMD.name, paramVector)) { - wbtError << "Failed to get vector field " << paramMD.name - << " from the struct at index " << paramMD.index << "."; - return false; - } - if (hasDynSizeColumns) { - if (!handleDynSizeColumns(paramMD.cols, paramVector.size())) { - return false; - } - } - ok = parameters.storeParameter(paramVector, paramMD); - } - break; - } - case ParameterType::STRUCT_STRING: { - if (metadataContainsScalarParam(paramMD)) { - std::string paramValue; - if (!getStringFieldAtIndex(paramMD.index, paramMD.name, paramValue)) { - wbtError << "Failed to get string field " << paramMD.name - << " from the struct at index " << paramMD.index << "."; - return false; - } - ok = parameters.storeParameter(paramValue, paramMD); - } - else { - wbtError << "Char arrays are not yet supported."; - return false; - } - break; - } - case ParameterType::STRUCT_CELL_INT: - case ParameterType::STRUCT_CELL_BOOL: - case ParameterType::STRUCT_CELL_DOUBLE: { - AnyCell cell; - std::vector paramVector; - if (!getCellFieldAtIndex(paramMD.index, paramMD.name, cell)) { - wbtError << "Failed to get cell field " << paramMD.name - << " from the struct at index " << paramMD.index << "."; - return false; - } - for (auto element : cell) { - double value; - if (!element->asDouble(value)) { - wbtError << "Failed to parse an element of the cell field " << paramMD.name - << " from the struct at index " << paramMD.index - << " as a double."; - return false; - } - paramVector.push_back(value); - } - if (hasDynSizeColumns) { - if (!handleDynSizeColumns(paramMD.cols, paramVector.size())) { - return false; - } - } - ok = parameters.storeParameter(paramVector, paramMD); - break; - } - case ParameterType::STRUCT_CELL_STRING: { - AnyCell cell; - std::vector paramVector; - if (!getCellFieldAtIndex(paramMD.index, paramMD.name, cell)) { - wbtError << "Failed to get cell field " << paramMD.name - << " from the struct at index " << paramMD.index << "."; - return false; - } - for (auto element : cell) { - std::string value; - if (!element->asString(value)) { - wbtError << "Failed to parse an element of the cell field " << paramMD.name - << " from the struct at index " << paramMD.index - << " as a string."; - return false; - } - paramVector.push_back(value); - } - if (hasDynSizeColumns) { - if (!handleDynSizeColumns(paramMD.cols, paramVector.size())) { - return false; - } - } - ok = parameters.storeParameter(paramVector, paramMD); - break; - } - } - - if (!ok) { - wbtError << "Failed to process parameter with index " << paramMD.index << "."; - return false; - } - } - - // Remove the metadata of the parameters already parsed. - // This is necessary for adding later more metadata and calling again this method - // (storing again an already stored parameter raises an error). - m_paramsMetadata.clear(); - - return true; -} - -bool SimulinkBlockInformation::setIOPortsData(const BlockInformation::IOData& ioData) -{ - // Set the number of input ports - if (!setNumberOfInputPorts(ioData.input.size())) { - wbtError << "Failed to set the number of input ports."; - return false; - } - - // Set the number of output ports - if (!setNumberOfOutputPorts(ioData.output.size())) { - wbtError << "Failed to set the number of output ports."; - return false; - } - - // Set the other input ports properties - for (const auto& portData : ioData.input) { - if (!updateInputPortData(portData)) { - return false; - } - } - - // Set the other output ports properties - for (const auto& portData : ioData.output) { - if (!updateOutputPortData(portData)) { - return false; - } - } - - return true; -} - -bool SimulinkBlockInformation::updateInputPortData(const BlockInformation::PortData& portData) -{ - // Get the port dimensions - const auto& portDimensions = std::get(portData); - if (portDimensions.size() > 2) { - wbtError << "Only vector (1D) and matrix (2D) input ports are supported."; - return false; - } - - // Get the port index and type - const auto& portIndex = std::get(portData); - const auto& portDataType = std::get(portData); - - bool ok = false; - - switch (portDimensions.size()) { - // 1D Vector - case 1: { - const VectorSize width = portDimensions.at(0); - ok = setInputPortVectorSize(portIndex, width) - && setInputPortType(portIndex, portDataType); - break; - } - // 2D Matrix - case 2: { - const Rows rows = portDimensions.at(0); - const Cols cols = portDimensions.at(1); - ok = setInputPortMatrixSize(portIndex, {rows, cols}) - && setInputPortType(portIndex, portDataType); - break; - } - } - - if (!ok) { - wbtError << "Failed to configure input port with index " << portIndex << "."; - return false; - } - - return true; -} - -bool SimulinkBlockInformation::updateOutputPortData(const BlockInformation::PortData& portData) -{ - // Get the port dimensions - const auto portDimensions = std::get(portData); - if (portDimensions.size() > 2) { - wbtError << "Only vector (1D) and matrix (2D) output ports are supported."; - return false; - } - - // Get the port index and type - const auto portIndex = std::get(portData); - const auto portDataType = std::get(portData); - - bool ok = false; - - switch (portDimensions.size()) { - // 1D Vector - case 1: { - const VectorSize width = portDimensions.at(0); - ok = setOutputPortVectorSize(portIndex, width) - && setOutputPortType(portIndex, portDataType); - break; - } - // 2D Matrix - case 2: { - const Rows rows = portDimensions.at(0); - const Cols cols = portDimensions.at(1); - ok = setOutputPortMatrixSize(portIndex, {rows, cols}) - && setOutputPortType(portIndex, portDataType); - break; - } - } - - if (!ok) { - wbtError << "Failed to configure output port with index " << portIndex << "."; - return false; - } - - return true; -} - -BlockInformation::PortData -SimulinkBlockInformation::getInputPortData(const BlockInformation::PortIndex idx) const -{ - const wbt::DataType dt = mapSimulinkToPortType(ssGetInputPortDataType(simstruct, idx)); - std::vector portDimension; - - switch (ssGetInputPortNumDimensions(simstruct, idx)) { - case 1: - portDimension = {ssGetInputPortWidth(simstruct, idx)}; - break; - case 2: { - const auto dims = ssGetInputPortDimensions(simstruct, idx); - portDimension = {dims[0], dims[1]}; - break; - } - } - - return std::make_tuple(idx, portDimension, dt); -} - -BlockInformation::PortData -SimulinkBlockInformation::getOutputPortData(const BlockInformation::PortIndex idx) const -{ - const wbt::DataType dt = mapSimulinkToPortType(ssGetOutputPortDataType(simstruct, idx)); - std::vector portDimension; - - switch (ssGetOutputPortNumDimensions(simstruct, idx)) { - case 1: - portDimension = {ssGetOutputPortWidth(simstruct, idx)}; - break; - case 2: { - const auto dims = ssGetOutputPortDimensions(simstruct, idx); - portDimension = {dims[0], dims[1]}; - break; - } - } - - return std::make_tuple(idx, portDimension, dt); -} diff --git a/toolbox/core/src/WBToolbox.cpp b/toolbox/core/src/WBToolbox.cpp deleted file mode 100644 index fb3946f57..000000000 --- a/toolbox/core/src/WBToolbox.cpp +++ /dev/null @@ -1,857 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#define S_FUNCTION_LEVEL 2 -#define S_FUNCTION_NAME WBToolbox - -#include "Core/Block.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/SimulinkBlockInformation.h" -#include "shlibpp/SharedLibrary.h" -#include "shlibpp/SharedLibraryClass.h" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -// ========= -// UTILITIES -// ========= - -using BlockFactory = shlibpp::SharedLibraryClassFactory; -using BlockFactoryPtr = std::shared_ptr; - -class BlockFactorySingleton -{ - using ClassFactoryName = std::string; - using ClassFactoryLibrary = std::string; - using BlockFactoryData = std::pair; - -public: - static std::string platformSpecificLibName(const std::string& library) - { -#if defined(_WIN32) - return library + "dll"; -#elif defined(__linux__) - return "lib" + library + ".so"; -#elif defined(__APPLE__) - return "lib" + library + ".dylib"; -#endif - } - - static BlockFactoryPtr getInstance(BlockFactoryData data) - { - static std::map factoryMap; - - ClassFactoryLibrary classFactoryLibrary = platformSpecificLibName(data.first); - ClassFactoryName& classFactoryName = data.second; - BlockFactoryData blockFactoryData = {classFactoryLibrary, classFactoryName}; - - // Clean possible leftovers - if (factoryMap.find(blockFactoryData) != factoryMap.end() - && !factoryMap[blockFactoryData]) { - factoryMap.erase(blockFactoryData); - } - - // Allocate the factory that loads the dll the first time - if (factoryMap.find(blockFactoryData) == factoryMap.end()) { - - // Allocate the factory - auto factory = std::make_shared(classFactoryLibrary.c_str(), - classFactoryName.c_str()); - if (!factory || !factory->isValid()) { - wbtError << "Failed to create factory"; - return {}; - } - - // Store it in the map - factoryMap.emplace(std::make_pair( - {classFactoryLibrary, classFactoryName}, std::move(factory))); - } - - if (!factoryMap[blockFactoryData] || !factoryMap[blockFactoryData]->isValid()) { - wbtError << "Failed to create factory"; - return {}; - } - - // Return the block factory - return factoryMap[blockFactoryData]; - } -}; - -const bool ForwardLogsToStdErr = true; - -static void catchLogMessages(bool status, SimStruct* S) -{ - // Initialize static buffers - const unsigned bufferLen = 1024; - - // Notify warnings - if (!wbt::Log::getSingleton().getWarnings().empty()) { - // Get the warnings - std::string warningMsg = wbt::Log::getSingleton().getWarnings(); - - // Trim the message if needed - if (warningMsg.length() >= bufferLen) { - warningMsg = warningMsg.substr(0, bufferLen - 1); - } - - // Forward to Simulink - char warningBuffer[bufferLen]; - sprintf(warningBuffer, "%s", warningMsg.c_str()); - ssWarning(S, warningBuffer); - - if (ForwardLogsToStdErr) { - fprintf(stderr, "%s", warningBuffer); - } - - // Clean the notified warnings - wbt::Log::getSingleton().clearWarnings(); - } - - // Notify errors - if (!status) { - // Get the errors - std::string errorMsg = wbt::Log::getSingleton().getErrors(); - - // Trim the message if needed - if (errorMsg.length() >= bufferLen) { - errorMsg = errorMsg.substr(0, bufferLen - 1); - } - - // Forward to Simulink - char errorBuffer[bufferLen]; - sprintf(errorBuffer, "%s", errorMsg.c_str()); - ssSetErrorStatus(S, errorBuffer); - - if (ForwardLogsToStdErr) { - fprintf(stderr, "%s", errorBuffer); - } - - // Clean the notified errors - wbt::Log::getSingleton().clearErrors(); - return; - } -} - -// ========== -// S-FUNCTION -// ========== - -// Function: MDL_CHECK_PARAMETERS -#define MDL_CHECK_PARAMETERS -#if defined(MDL_CHECK_PARAMETERS) && defined(MATLAB_MEX_FILE) -static void mdlCheckParameters(SimStruct* S) -{ - UNUSED_ARG(S); - // TODO: still to find a way to call Block implementation -} -#endif /*MDL_CHECK_PARAMETERS*/ - -#define MDL_SET_INPUT_PORT_DIMENSION_INFO -static void mdlSetInputPortDimensionInfo(SimStruct* S, int_T port, const DimsInfo_T* dimsInfo) -{ - // If the port is set as dynamic and the proposed dimension is not dynamic (hence calculated by - // the signal propagation) accept it - if (ssGetInputPortWidth(S, port) == DYNAMICALLY_SIZED) { - if (dimsInfo->width != DYNAMICALLY_SIZED) { - if (!ssSetInputPortDimensionInfo(S, port, dimsInfo)) { - wbtError << "Failed to set proposed sizes."; - catchLogMessages(false, S); - return; - } - } - } -} - -#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO -static void mdlSetOutputPortDimensionInfo(SimStruct* S, int_T port, const DimsInfo_T* dimsInfo) -{ - // If the port is set as dynamic and the proposed dimension is not dynamic (hence calculated by - // the signal propagation) accept it - if (ssGetOutputPortWidth(S, port) == DYNAMICALLY_SIZED) { - if (dimsInfo->width != DYNAMICALLY_SIZED) { - if (!ssSetOutputPortDimensionInfo(S, port, dimsInfo)) { - wbtError << "Failed to set proposed sizes."; - catchLogMessages(false, S); - return; - } - } - } -} - -// Function: mdlInitializeSizes =============================================== -// 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) -{ - // Initialize the Log singleton - wbt::Log::getSingleton().clear(); - - if (ssGetSFcnParamsCount(S) < 1) { - wbtError << "The block type parameter must be specified"; - catchLogMessages(false, S); - return; - } - - // Get the class name and the library name from the parameter - const std::string className(mxArrayToString(ssGetSFcnParam(S, 0))); - const std::string blockLibraryName(mxArrayToString(ssGetSFcnParam(S, 1))); - - // Get the block factory - BlockFactoryPtr factory = BlockFactorySingleton::getInstance({blockLibraryName, className}); - - if (!factory) { - wbtError << "Failed to get factory object"; - catchLogMessages(false, S); - return; - } - - if (!factory->isValid()) { - wbtError << "Factory error (" << static_cast(factory->getStatus()) - << "): " << factory->getError().c_str(); - catchLogMessages(false, S); - return; - } - - // Allocate the block from the factory. Since this object is supposed to be deleted - // by the end of this function scope, SharedLibraryClass can be used and provides RAII. - shlibpp::SharedLibraryClass block(*factory); - - // Notify errors - if (!block.isValid()) { - wbtError << "Could not create an object of type " + className; - catchLogMessages(false, S); - return; - } - - // We cannot save data in PWork during the initializeSizes phase. - - // Two PWorks: - // 0: pointer to a Block implementation - // 1: pointer to a BlockInformation implementation - ssSetNumPWork(S, 2); - - // Setup the block parameters' properties - ssSetNumSFcnParams(S, block->numberOfParameters()); - ssSetSFcnParamTunable(S, 0, false); - for (unsigned i = 0; i < ssGetNumSFcnParams(S); ++i) { - bool tunable = false; - block->parameterAtIndexIsTunable(i, tunable); - ssSetSFcnParamTunable(S, i, tunable); - } - -#if defined(MATLAB_MEX_FILE) - if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { - mdlCheckParameters(S); - if (ssGetErrorStatus(S)) { - return; - } - } - else { - int_T numOfExpectedParams = ssGetNumSFcnParams(S); - int_T numOfBlockParams = ssGetSFcnParamsCount(S); - wbtError << "Number of parameters (" << numOfBlockParams - << ") different from those expected (" << numOfExpectedParams << ")"; - catchLogMessages(false, S); - return; - } -#endif - - wbt::SimulinkBlockInformation blockInfo(S); - bool ok = block->configureSizeAndPorts(&blockInfo); - catchLogMessages(ok, S); - - if (!ok) { - return; - } - - 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); - - ssSetSimStateCompliance(S, USE_CUSTOM_SIM_STATE); //?? - - ssSetNumDiscStates(S, block->numberOfDiscreteStates()); - ssSetNumContStates(S, 0); // block->numberOfContinuousStates()); - - uint_T options = SS_OPTION_WORKS_WITH_CODE_REUSE | SS_OPTION_EXCEPTION_FREE_CODE - | SS_OPTION_ALLOW_INPUT_SCALAR_EXPANSION | SS_OPTION_USE_TLC_WITH_ACCELERATOR - | SS_OPTION_CALL_TERMINATE_ON_EXIT; - // also ? - // SS_OPTION_RUNTIME_EXCEPTION_FREE_CODE - - std::vector additionalOptions = block->additionalBlockOptions(); - - for (const auto& additionalOption : additionalOptions) { - double option; - if (!blockInfo.optionFromKey(additionalOption, option)) { - wbtError << "Failed to get option from key."; - catchLogMessages(false, S); - return; - } - // Store the parsed option - options |= static_cast(option); - } - - // Set the options - ssSetOptions(S, options); -} - -// Function: mdlInitializeSampleTimes ========================================= -// Abstract: -// 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) -{ - ssSetSampleTime(S, 0, INHERITED_SAMPLE_TIME); - ssSetOffsetTime(S, 0, 0.0); - ssSetModelReferenceSampleTimeDefaultInheritance(S); -} - -// Function: mdlStart ======================================================= -// Abstract: -// This function is called once at start of model execution. If you -// have states that should be initialized once, this is the place -// to do it. -#define MDL_START -static void mdlStart(SimStruct* S) -{ - // Get the class name and the library name from the parameter - const std::string className(mxArrayToString(ssGetSFcnParam(S, 0))); - const std::string blockLibraryName(mxArrayToString(ssGetSFcnParam(S, 1))); - - // Get the block factory - BlockFactoryPtr factory = BlockFactorySingleton::getInstance({blockLibraryName, className}); - - if (!factory) { - wbtError << "Failed to get factory object"; - catchLogMessages(false, S); - return; - } - - if (!factory->isValid()) { - wbtError << "Factory error (" << static_cast(factory->getStatus()) - << "): " << factory->getError().c_str(); - catchLogMessages(false, S); - return; - } - - // Allocate the block from the factory and store its pointer in the PWork - wbt::Block* block = factory->create(); - ssSetPWorkValue(S, 0, block); - - // Allocate the BlockInformation object and store its pointer in the PWork - wbt::BlockInformation* blockInfo = new wbt::SimulinkBlockInformation(S); - ssSetPWorkValue(S, 1, blockInfo); - - if (!block || !blockInfo) { - wbtError << "Failed to create objects before storing them in the PWork."; - catchLogMessages(false, S); - return; - } - - // Call the initialize() method - bool ok = block->initialize(blockInfo); - catchLogMessages(ok, S); -} - -#define MDL_UPDATE -#if defined(MDL_UPDATE) && defined(MATLAB_MEX_FILE) -static void mdlUpdate(SimStruct* S, int_T tid) -{ - UNUSED_ARG(tid); - if (ssGetNumPWork(S) != 2) { - wbtError << "PWork should contain two elements."; - catchLogMessages(false, S); - return; - } - - // Get the Block object - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - // Get the SimulinkBlockInformation object - wbt::SimulinkBlockInformation* blockInfo; - blockInfo = static_cast(ssGetPWorkValue(S, 1)); - - if (!block || !blockInfo) { - wbtError << "Failed to get pointers from the PWork vector."; - catchLogMessages(false, S); - return; - } - - // Call the updateDiscreteState() method - bool ok = block->updateDiscreteState(blockInfo); - catchLogMessages(ok, S); -} -#endif - -// 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) -{ - if (ssGetNumPWork(S) != 2) { - wbtError << "PWork should contain two elements."; - catchLogMessages(false, S); - return; - } - - // Get the Block object - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - // Get the SimulinkBlockInformation object - wbt::SimulinkBlockInformation* blockInfo; - blockInfo = static_cast(ssGetPWorkValue(S, 1)); - - if (!block || !blockInfo) { - wbtError << "Failed to get pointers from the PWork vector."; - catchLogMessages(false, S); - return; - } - - // Call the initializeInitialConditions() method - bool ok = block->initializeInitialConditions(blockInfo); - catchLogMessages(ok, S); -} -#endif - -#define MDL_DERIVATIVES -#if defined(MDL_DERIVATIVES) && defined(MATLAB_MEX_FILE) -static void mdlDerivatives(SimStruct* /*S*/) -{ - /* Add mdlDerivatives code here */ -} -#endif - -// Function: mdlOutputs ======================================================= -// Abstract: -// In this function, you compute the outputs of your S-function -// block. -static void mdlOutputs(SimStruct* S, int_T tid) -{ - UNUSED_ARG(tid); - if (ssGetNumPWork(S) != 2) { - wbtError << "PWork should contain two elements."; - catchLogMessages(false, S); - return; - } - - // Get the Block object - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - // Get the SimulinkBlockInformation object - wbt::SimulinkBlockInformation* blockInfo; - blockInfo = static_cast(ssGetPWorkValue(S, 1)); - - if (!block || !blockInfo) { - wbtError << "Failed to get pointers from the PWork vector."; - catchLogMessages(false, S); - return; - } - - // Call the output() method - bool ok = block->output(blockInfo); - catchLogMessages(ok, S); -} - -static void mdlTerminate(SimStruct* S) -{ - if (!ssGetPWork(S)) { - return; - } - - if (ssGetNumPWork(S) != 2) { - wbtError << "PWork should contain two elements."; - catchLogMessages(false, S); - return; - } - - // Get the Block object - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - // Get the SimulinkBlockInformation object - wbt::SimulinkBlockInformation* blockInfo; - blockInfo = static_cast(ssGetPWorkValue(S, 1)); - - if (block) { - if (!block->terminate(blockInfo)) { - wbtError << "Failed to terminate block."; - catchLogMessages(false, S); - } - } - else { - wbtWarning << "Failed to get Block pointer from the PWork vector." << std::endl - << "Could't terminate block"; - } - - // Get the class name and the library name from the parameter - const std::string className(mxArrayToString(ssGetSFcnParam(S, 0))); - const std::string blockLibraryName(mxArrayToString(ssGetSFcnParam(S, 1))); - - // Get the block factory - BlockFactoryPtr factory = BlockFactorySingleton::getInstance({blockLibraryName, className}); - - if (!factory) { - wbtError << "Failed to get factory object"; - catchLogMessages(false, S); - return; - } - - if (!factory->isValid()) { - wbtError << "Factory error (" << static_cast(factory->getStatus()) - << "): " << factory->getError().c_str(); - catchLogMessages(false, S); - return; - } - - // Delete the resources allocated in the PWork vector - delete blockInfo; - - factory->destroy(block); - - // Clean the PWork vector - ssSetPWorkValue(S, 0, nullptr); - ssSetPWorkValue(S, 1, nullptr); -} - -#if defined(MATLAB_MEX_FILE) -#define MDL_RTW - -template -std::vector toRTWNumericVector(const std::vector& vectorInput) -{ - std::vector output; - output.reserve(vectorInput.size()); - - output.assign(vectorInput.begin(), vectorInput.end()); - return output; -} - -std::string toRTWStringVector(const std::vector& stringInput) -{ - std::string output; - - for (unsigned i = 0; i < stringInput.size(); ++i) { - if (i == 0) { - output += "[\"" + stringInput[i] + "\""; - } - else { - output += ", \"" + stringInput[i] + "\""; - } - } - output += "]"; - return output; -} - -const std::pair parameterTypeToString(const wbt::ParameterType& type) -{ - switch (type) { - case wbt::ParameterType::INT: - return {"ParameterType::INT", "int"}; - case wbt::ParameterType::BOOL: - return {"ParameterType::BOOL", "bool"}; - case wbt::ParameterType::DOUBLE: - return {"ParameterType::DOUBLE", "double"}; - case wbt::ParameterType::STRING: - return {"ParameterType::STRING", "std::string"}; - case wbt::ParameterType::CELL_INT: - return {"ParameterType::CELL_INT", "int"}; - case wbt::ParameterType::CELL_BOOL: - return {"ParameterType::CELL_BOOL", "bool"}; - case wbt::ParameterType::CELL_DOUBLE: - return {"ParameterType::CELL_DOUBLE", "double"}; - case wbt::ParameterType::CELL_STRING: - return {"ParameterType::CELL_STRING", "std::string"}; - case wbt::ParameterType::STRUCT_INT: - return {"ParameterType::STRUCT_INT", "int"}; - case wbt::ParameterType::STRUCT_BOOL: - return {"ParameterType::STRUCT_BOOL", "bool"}; - case wbt::ParameterType::STRUCT_DOUBLE: - return {"ParameterType::STRUCT_DOUBLE", "double"}; - case wbt::ParameterType::STRUCT_STRING: - return {"ParameterType::STRUCT_STRING", "std::string"}; - case wbt::ParameterType::STRUCT_CELL_INT: - return {"ParameterType::STRUCT_CELL_INT", "int"}; - case wbt::ParameterType::STRUCT_CELL_BOOL: - return {"ParameterType::STRUCT_CELL_BOOL", "bool"}; - case wbt::ParameterType::STRUCT_CELL_DOUBLE: - return {"ParameterType::STRUCT_CELL_DOUBLE", "double"}; - case wbt::ParameterType::STRUCT_CELL_STRING: - return {"ParameterType::STRUCT_CELL_STRING", "std::string"}; - } -} - -template -bool writeParameterToRTW(const wbt::Parameter param, SimStruct* S) -{ - if (param.getMetadata().cols == wbt::ParameterMetadata::DynamicSize - || param.getMetadata().rows == wbt::ParameterMetadata::DynamicSize) { - wbtError << "Storing in the rtw file dynamically-sized parameters is not supported."; - return false; - } - - if (param.isScalar()) { - return ssWriteRTWParamSettings( - S, - 8, - SSWRITE_VALUE_NUM, - "index", - static_cast(param.getMetadata().index), - SSWRITE_VALUE_QSTR, - "name", - param.getMetadata().name.c_str(), - SSWRITE_VALUE_NUM, - "isScalar", - static_cast(param.isScalar()), - SSWRITE_VALUE_NUM, - "rows", - static_cast(param.getMetadata().rows), - SSWRITE_VALUE_NUM, - "cols", - static_cast(param.getMetadata().cols), - SSWRITE_VALUE_QSTR, - "type", - parameterTypeToString(param.getMetadata().type).first.c_str(), - SSWRITE_VALUE_QSTR, - "storage", - parameterTypeToString(param.getMetadata().type).second.c_str(), - SSWRITE_VALUE_NUM, - "valueScalar", - static_cast(param.getScalarParameter())); - } - else { - const std::vector vectorRealT = toRTWNumericVector(param.getVectorParameter()); - return ssWriteRTWParamSettings( - S, - 8, - SSWRITE_VALUE_NUM, - "index", - static_cast(param.getMetadata().index), - SSWRITE_VALUE_QSTR, - "name", - param.getMetadata().name.c_str(), - SSWRITE_VALUE_NUM, - "isScalar", - static_cast(param.isScalar()), - SSWRITE_VALUE_NUM, - "rows", - static_cast(param.getMetadata().rows), - SSWRITE_VALUE_NUM, - "cols", - static_cast(param.getMetadata().cols), - SSWRITE_VALUE_QSTR, - "type", - parameterTypeToString(param.getMetadata().type).first.c_str(), - SSWRITE_VALUE_QSTR, - "storage", - parameterTypeToString(param.getMetadata().type).second.c_str(), - SSWRITE_VALUE_VECT, - "valueVector", - vectorRealT.data(), - param.getVectorParameter().size()); - } -} - -// Specialize the template for std::string -template <> -bool writeParameterToRTW(const wbt::Parameter param, SimStruct* S) -{ - if (param.getMetadata().cols == wbt::ParameterMetadata::DynamicSize - || param.getMetadata().rows == wbt::ParameterMetadata::DynamicSize) { - wbtError << "Storing in the rtw file dynamically-sized parameters is not supported."; - return false; - } - - if (param.isScalar()) { - return ssWriteRTWParamSettings( - S, - 8, - SSWRITE_VALUE_NUM, - "index", - static_cast(param.getMetadata().index), - SSWRITE_VALUE_QSTR, - "name", - param.getMetadata().name.c_str(), - SSWRITE_VALUE_NUM, - "isScalar", - static_cast(param.isScalar()), - SSWRITE_VALUE_NUM, - "rows", - static_cast(param.getMetadata().rows), - SSWRITE_VALUE_NUM, - "cols", - static_cast(param.getMetadata().cols), - SSWRITE_VALUE_QSTR, - "type", - parameterTypeToString(param.getMetadata().type).first.c_str(), - SSWRITE_VALUE_QSTR, - "storage", - parameterTypeToString(param.getMetadata().type).second.c_str(), - SSWRITE_VALUE_QSTR, - "valueScalar", - param.getScalarParameter().c_str()); - } - else { - const std::string serializedVectorOfStrings = toRTWStringVector(param.getVectorParameter()); - return ssWriteRTWParamSettings( - S, - 8, - SSWRITE_VALUE_NUM, - "index", - static_cast(param.getMetadata().index), - SSWRITE_VALUE_QSTR, - "name", - param.getMetadata().name.c_str(), - SSWRITE_VALUE_NUM, - "isScalar", - static_cast(param.isScalar()), - SSWRITE_VALUE_NUM, - "rows", - static_cast(param.getMetadata().rows), - SSWRITE_VALUE_NUM, - "cols", - static_cast(param.getMetadata().cols), - SSWRITE_VALUE_QSTR, - "type", - parameterTypeToString(param.getMetadata().type).first.c_str(), - SSWRITE_VALUE_QSTR, - "storage", - parameterTypeToString(param.getMetadata().type).second.c_str(), - SSWRITE_VALUE_VECT_STR, - "valueVector", - serializedVectorOfStrings.c_str(), - param.getMetadata().cols); - } -} - -bool writeRTW(SimStruct* S, const wbt::Parameters& params) -{ - // RTW Parameters record metadata - // ============================== - - // The first entry in the parameter record (SFcnParamSettings[0]) contains - // information useful to parse the others - - // Get the number or parameters - const unsigned numberOfParameters = params.getNumberOfParameters(); - - // Get the class name - std::string className; - params.getParameter("className", className); - - // Create the record - ssWriteRTWParamSettings(S, - 2, - SSWRITE_VALUE_NUM, - "numberOfParameters", - static_cast(numberOfParameters), - SSWRITE_VALUE_QSTR, - "className", - className.c_str()); - - // RTW Parameters - // ============== - - bool ok = true; - - for (const auto& param : params.getIntParameters()) { - ok = ok && writeParameterToRTW(param, S); - } - - for (const auto& param : params.getBoolParameters()) { - ok = ok && writeParameterToRTW(param, S); - } - - for (const auto& param : params.getDoubleParameters()) { - ok = ok && writeParameterToRTW(param, S); - } - - for (const auto& param : params.getStringParameters()) { - ok = ok && writeParameterToRTW(param, S); - } - - if (!ok) { - wbtError << "Failed to write parameters to RTW file."; - catchLogMessages(false, S); - return false; - } - - return true; -} - -static void mdlRTW(SimStruct* S) -{ - if (ssGetNumPWork(S) > 0 && ssGetPWork(S)) { - - // Get the block object from the PWork - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - - bool ok; - wbt::Parameters params; - - if (!block) { - wbtError << "Unable to get the class from the PWork vector."; - catchLogMessages(false, S); - return; - } - - // Get the parameters from the block - ok = block->getParameters(params); - catchLogMessages(ok, S); - if (!ok) { - wbtError << "Failed to get parameters from the block during the code " - << "generation process"; - return; - } - - // Use parameters metadata to populate the rtw file used by the coder - ok = writeRTW(S, params); - catchLogMessages(ok, S); - if (!ok) { - wbtError << "Failed to write parameters to the RTW file during the code " - << "generation process"; - return; - } - - // Store the PWork vector in the rtw file - ok = ssWriteRTWWorkVect(S, "PWork", 1, "blockPWork", ssGetNumPWork(S)); - catchLogMessages(ok, S); - if (!ok) { - wbtError << "Failed to store the PWork vector during the code " - << "generation process"; - return; - } - } -} -#endif - -// Required S-function trailer -#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ -#include /* MEX-file interface mechanism */ -#else -#include "cg_sfun.h" /* Code generation registration function */ -#endif diff --git a/toolbox/library/CMakeLists.txt b/toolbox/library/CMakeLists.txt index 6c14d8001..b8dace97e 100644 --- a/toolbox/library/CMakeLists.txt +++ b/toolbox/library/CMakeLists.txt @@ -2,235 +2,88 @@ # This software may be modified and distributed under the terms of the # GNU Lesser General Public License v2.1 or any later version. -# ================= -# FIND DEPENDENCIES -# ================= +# ===================== +# OPTIONAL DEPENDENCIES +# ===================== -find_package(YARP 3.0.0 REQUIRED) - -# Fail if YARP is not compiled as shared library -# see https://github.com/robotology/codyco-modules/issues/44 -if(NOT ${YARP_IS_SHARED_LIBRARY}) - message(FATAL_ERROR "YARP was found, but it was compiled as static library. A shared library version of YARP is required.") +find_package(ICUB) +if(NOT WBT_USES_ICUB) + option(WBT_USES_ICUB "Build blocks that depend on iCub library" ${ICUB_FOUND}) endif() -find_package(iDynTree 0.7.2 REQUIRED) - -# This find loads Eigen. -# It would be nice using the EigenConfig shipped with eigen package, but -# on Ubuntu 16.04 eigen is old and provides only FindEigen. -find_package(Eigen3 REQUIRED) - -# ============== -# WBTOOLBOX BASE -# ============== - -add_library(LibraryBase - src/RobotInterface.cpp - src/Configuration.cpp - src/WholeBodySingleton.cpp - src/WBBlock.cpp - include/Base/RobotInterface.h - include/Base/Configuration.h - include/Base/WholeBodySingleton.h - include/Base/WBBlock.h) - -target_link_libraries(LibraryBase - WBToolbox::Core - YARP::YARP_OS - YARP::YARP_init - YARP::YARP_dev - iDynTree::idyntree-model - iDynTree::idyntree-modelio-urdf - iDynTree::idyntree-high-level) - -target_include_directories(LibraryBase PUBLIC - $ - $ - $) - -install( - DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/include/Base - DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/WBToolbox) - -# ================= -# WBTOOLBOX LIBRARY -# ================= - -set(WBTOOLBOXLIBRARY_EXT_LIBRARIES "") - -# 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) - find_package(ICUB REQUIRED) - - configure_block(BLOCK_NAME "Minimum Jerk Trajectory Generator" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/MinimumJerkTrajectoryGenerator.cpp - HEADERS include/Block/MinimumJerkTrajectoryGenerator.h) - - configure_block(BLOCK_NAME "Discrete Filter" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/DiscreteFilter.cpp - HEADERS include/Block/DiscreteFilter.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() -endif() - -# Search qpOASES and enable block which depends on it find_package(qpOASES) if(NOT WBT_USES_QPOASES) - option(WBT_USES_QPOASES "Compile also qpOASES-related components" ${qpOASES_FOUND}) + option(WBT_USES_QPOASES "Build blocks that depend on qpOASES library" ${qpOASES_FOUND}) endif() -configure_block(BLOCK_NAME "Yarp Read" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/YarpRead.cpp - HEADERS include/Block/YarpRead.h) +# =================== +# BLOCKFACTORY PLUGIN +# =================== + +# YARP blocks +list(APPEND WBT_BLOCKS "YarpRead") +list(APPEND WBT_BLOCKS "YarpWrite") +list(APPEND WBT_BLOCKS "YarpClock") +list(APPEND WBT_BLOCKS "GetLimits") +list(APPEND WBT_BLOCKS "SetReferences") +list(APPEND WBT_BLOCKS "GetMeasurement") +list(APPEND WBT_BLOCKS "SetMotorParameters") +list(APPEND WBT_BLOCKS "SimulatorSynchronizer") + +# iDynTree blocks +list(APPEND WBT_BLOCKS "MassMatrix") +list(APPEND WBT_BLOCKS "InverseDynamics") +list(APPEND WBT_BLOCKS "DotJNu") +list(APPEND WBT_BLOCKS "Jacobian") +list(APPEND WBT_BLOCKS "ForwardKinematics") +list(APPEND WBT_BLOCKS "RelativeTransform") +list(APPEND WBT_BLOCKS "CentroidalMomentum") + +# Other +list(APPEND WBT_BLOCKS "RealTimeSynchronizer") -configure_block(BLOCK_NAME "Yarp Write" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/YarpWrite.cpp - HEADERS include/Block/YarpWrite.h) - -configure_block(BLOCK_NAME "Real Time Synchronizer" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/RealTimeSynchronizer.cpp - HEADERS include/Block/RealTimeSynchronizer.h) - -configure_block(BLOCK_NAME "Yarp Clock" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/YarpClock.cpp - HEADERS include/Block/YarpClock.h) +if(WBT_USES_ICUB) + list(APPEND WBT_BLOCKS "DiscreteFilter") + list(APPEND WBT_BLOCKS "MinimumJerkTrajectoryGenerator") +endif() if(WBT_USES_QPOASES) - configure_block(BLOCK_NAME "qpOASES" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/QpOases.cpp - HEADERS include/Block/QpOases.h) + list(APPEND WBT_BLOCKS "QpOases") endif() -configure_block(BLOCK_NAME "Simulator Synchronizer" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/SimulatorSynchronizer.cpp - HEADERS include/Block/SimulatorSynchronizer.h) -add_library(WBToolbox::ClockRPC ALIAS ClockRpc) -list(APPEND WBTOOLBOXLIBRARY_EXT_LIBRARIES "ClockRpc") - -configure_block(BLOCK_NAME "Mass Matrix" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/MassMatrix.cpp - HEADERS include/Block/MassMatrix.h) - -configure_block(BLOCK_NAME "Inverse Dynamics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/InverseDynamics.cpp - HEADERS include/Block/InverseDynamics.h) - -configure_block(BLOCK_NAME "Centroidal Momentum" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/CentroidalMomentum.cpp - HEADERS include/Block/CentroidalMomentum.h) - -configure_block(BLOCK_NAME "Forward Kinematics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/ForwardKinematics.cpp - HEADERS include/Block/ForwardKinematics.h) - -configure_block(BLOCK_NAME "Jacobian" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/Jacobian.cpp - HEADERS include/Block/Jacobian.h) - -configure_block(BLOCK_NAME "DotJ Nu" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/DotJNu.cpp - HEADERS include/Block/DotJNu.h) - -configure_block(BLOCK_NAME "Relative Transform" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/RelativeTransform.cpp - HEADERS include/Block/RelativeTransform.h) - -configure_block(BLOCK_NAME "Set References" - GROUP "Actuators" - LIST_PREFIX WBT - SOURCES src/SetReferences.cpp - HEADERS include/Block/SetReferences.h) - -configure_block(BLOCK_NAME "Set Motor Parameters" - GROUP "Actuators" - LIST_PREFIX WBT - SOURCES src/SetMotorParameters.cpp - HEADERS include/Block/SetMotorParameters.h) - -configure_block(BLOCK_NAME "Get Measurement" - GROUP "State" - LIST_PREFIX WBT - SOURCES src/GetMeasurement.cpp - HEADERS include/Block/GetMeasurement.h) +# Include CMake helpers provided by BlockFactory +include(BlockFactoryPlugin) -configure_block(BLOCK_NAME "Get Limits" - GROUP "State" - LIST_PREFIX WBT - SOURCES src/GetLimits.cpp - HEADERS include/Block/GetLimits.h) +# Register the blocks +foreach(block ${WBT_BLOCKS}) + register_blockfactory_block( + BLOCK_NAME ${block} + PLUGIN_NAME WBToolbox + SOURCES src/${block}.cpp + HEADERS include/WBToolbox/Block/${block}.h) +endforeach() -get_property(ALL_LIBRARY_HEADERS GLOBAL PROPERTY WBT_HEADERS) -get_property(ALL_LIBRARY_SOURCES GLOBAL PROPERTY WBT_SOURCES) +# Create the target of the plugin library +add_blockfactory_plugin(WBToolbox + EXTRA_SOURCES src/Factory.cpp) +add_library(WBToolbox::Plugin ALIAS WBToolbox) -add_library(Library SHARED - src/Factory.cpp - ${ALL_LIBRARY_HEADERS} - ${ALL_LIBRARY_SOURCES}) -add_library(WBToolbox::Library ALIAS Library) +# Set public headers +get_property(WBT_BLOCKS_HEADERS GLOBAL PROPERTY WBToolbox_HEADERS) +set_target_properties(WBToolbox PROPERTIES + PUBLIC_HEADER "${WBT_BLOCKS_HEADERS}") if(MSVC) # Import math symbols from standard cmath - target_compile_definitions(Library PRIVATE "_USE_MATH_DEFINES") + target_compile_definitions(WBToolbox PRIVATE "_USE_MATH_DEFINES") endif() -target_include_directories(Library PUBLIC - $ - $) - -target_include_directories(Library SYSTEM PRIVATE ${EIGEN3_INCLUDE_DIR}) - -set_target_properties(Library PROPERTIES - PUBLIC_HEADER "${ALL_LIBRARY_HEADERS}" - OUTPUT_NAME "WBToolboxLibrary") +# Configure the include directories +target_include_directories(WBToolbox PUBLIC + $ + $) +# Main dependencies list(APPEND WBTOOLBOXLIBRARY_EXT_LIBRARIES YARP::YARP_OS YARP::YARP_dev @@ -240,50 +93,31 @@ list(APPEND WBTOOLBOXLIBRARY_EXT_LIBRARIES iDynTree::idyntree-modelio-urdf iDynTree::idyntree-high-level) -target_link_libraries(Library PUBLIC LibraryBase) -target_link_libraries(Library PRIVATE WBToolbox::Core shlibpp::shlibpp ${WBTOOLBOXLIBRARY_EXT_LIBRARIES}) +# Additional dependency of the SimulatorSynchronizer block +add_library(WBToolbox::ClockRPC ALIAS ClockRpc) +list(APPEND WBTOOLBOXLIBRARY_EXT_LIBRARIES "ClockRpc") + +# Link the plugin with the WBToolboxBase library and +# all the external dependencies +target_link_libraries(WBToolbox + PUBLIC WBToolboxBase + PRIVATE ${WBTOOLBOXLIBRARY_EXT_LIBRARIES}) if(WBT_USES_ICUB) - target_compile_definitions(Library PRIVATE "WBT_USES_ICUB") - target_link_libraries(Library PUBLIC ctrlLib) + target_compile_definitions(WBToolbox PRIVATE "WBT_USES_ICUB") + target_link_libraries(WBToolbox PRIVATE ctrlLib) # Manually include iCub include folders. They are not exported by the linked library. - target_include_directories(Library PRIVATE ${ctrlLib_INCLUDE_DIRS}) - -# if (${ICUB_USE_IPOPT}) -# target_link_libraries(WBToolboxLibrary PUBLIC iKin) -# include_directories(${iKin_INCLUDE_DIRS}) -# endif() + target_include_directories(WBToolbox PRIVATE ${ctrlLib_INCLUDE_DIRS}) endif() if(WBT_USES_QPOASES) - target_compile_definitions(Library PUBLIC "WBT_USES_QPOASES") - target_link_libraries(Library PUBLIC ${qpOASES_LIBRARIES}) + target_compile_definitions(WBToolbox PRIVATE "WBT_USES_QPOASES") + target_link_libraries(WBToolbox PRIVATE ${qpOASES_LIBRARIES}) # Manually include qpOASES include folders. They are not exported by the linked library. - target_include_directories(Library PRIVATE ${qpOASES_INCLUDE_DIRS}) -endif() - -install( - TARGETS LibraryBase Library - EXPORT WBToolboxLibraryExport - LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} - ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} - RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} - PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/WBToolbox/Block) - -if(WBT_USES_ICUB) - list(APPEND WBTOOLBOX_LIBRARY_PUBLIC_DEPENDS iCub) -endif() -if(WBT_USES_QPOASES) - list(APPEND WBTOOLBOX_LIBRARY_PUBLIC_DEPENDS qpOASES) + target_include_directories(WBToolbox PRIVATE ${qpOASES_INCLUDE_DIRS}) endif() -install_basic_package_files(WBToolboxLibrary - VERSION ${PROJECT_VERSION} - COMPATIBILITY AnyNewerVersion - EXPORT WBToolboxLibraryExport - FIRST_TARGET LibraryBase - DEPENDENCIES YARP iDynTree ${WBTOOLBOX_LIBRARY_PUBLIC_DEPENDS} ToolboxCore # TODO: Eigen3 - NAMESPACE WBToolbox:: - NO_CHECK_REQUIRED_COMPONENTS_MACRO) +# Install the plugin +install_blockfactory_plugin(WBToolbox) diff --git a/toolbox/library/include/Block/CentroidalMomentum.h b/toolbox/library/include/Block/CentroidalMomentum.h deleted file mode 100644 index ca700619d..000000000 --- a/toolbox/library/include/Block/CentroidalMomentum.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_CENTROIDALMOMENTUM_H -#define WBT_CENTROIDALMOMENTUM_H - -#include "Base/WBBlock.h" - -#include -#include - -namespace wbt { - class BlockInformation; - class CentroidalMomentum; -} // namespace wbt - -/** - * @brief The wbt::CentroidalMomentum class - */ -class wbt::CentroidalMomentum final : public wbt::WBBlock -{ -private: - class impl; - std::unique_ptr pImpl; - -public: - CentroidalMomentum(); - ~CentroidalMomentum() override; - - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; -}; - -#endif // WBT_CENTROIDALMOMENTUM_H diff --git a/toolbox/library/include/Block/InverseDynamics.h b/toolbox/library/include/Block/InverseDynamics.h deleted file mode 100644 index 98d5e0bd9..000000000 --- a/toolbox/library/include/Block/InverseDynamics.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_INVERSEDYNAMICS_H -#define WBT_INVERSEDYNAMICS_H - -#include "Base/WBBlock.h" - -#include -#include - -namespace wbt { - class BlockInformation; - class InverseDynamics; -} // namespace wbt - -/** - * @brief The wbt::InverseDynamics class - */ -class wbt::InverseDynamics final : public wbt::WBBlock -{ -private: - class impl; - std::unique_ptr pImpl; - -public: - InverseDynamics(); - ~InverseDynamics() override; - - unsigned numberOfParameters() override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; -}; - -#endif // WBT_INVERSEDYNAMICS_H diff --git a/toolbox/library/include/Block/MassMatrix.h b/toolbox/library/include/Block/MassMatrix.h deleted file mode 100644 index 151c4a397..000000000 --- a/toolbox/library/include/Block/MassMatrix.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_MASSMATRIX_H -#define WBT_MASSMATRIX_H - -#include "Base/WBBlock.h" - -#include -#include - -namespace wbt { - class BlockInformation; - class MassMatrix; -} // namespace wbt - -/** - * @brief The wbt::MassMatrix class - */ -class wbt::MassMatrix final : public wbt::WBBlock -{ -private: - class impl; - std::unique_ptr pImpl; - -public: - MassMatrix(); - ~MassMatrix() override; - - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; -}; - -#endif // WBT_MASSMATRIX_H diff --git a/toolbox/library/include/Block/YarpClock.h b/toolbox/library/include/Block/YarpClock.h deleted file mode 100644 index 43f5baa72..000000000 --- a/toolbox/library/include/Block/YarpClock.h +++ /dev/null @@ -1,37 +0,0 @@ -/* - * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) - * All rights reserved. - * - * This software may be modified and distributed under the terms of the - * GNU Lesser General Public License v2.1 or any later version. - */ - -#ifndef WBT_YARPCLOCK_H -#define WBT_YARPCLOCK_H - -#include "Core/Block.h" - -#include - -namespace wbt { - class BlockInformation; - class YarpClock; -} // namespace wbt - -/** - * @brief The wbt::YarpClock class - */ -class wbt::YarpClock final : public wbt::Block -{ -public: - YarpClock() = default; - ~YarpClock() override = default; - - unsigned numberOfParameters() override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; -}; - -#endif // WBT_YARPCLOCK_H diff --git a/toolbox/library/include/WBToolbox/Block/CentroidalMomentum.h b/toolbox/library/include/WBToolbox/Block/CentroidalMomentum.h new file mode 100644 index 000000000..4a5c8a237 --- /dev/null +++ b/toolbox/library/include/WBToolbox/Block/CentroidalMomentum.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef WBT_CENTROIDALMOMENTUM_H +#define WBT_CENTROIDALMOMENTUM_H + +#include "WBToolbox/Base/WBBlock.h" + +#include +#include + +namespace wbt { + namespace block { + class CentroidalMomentum; + } // namespace block +} // namespace wbt + +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + +/** + * @brief The wbt::CentroidalMomentum class + */ +class wbt::block::CentroidalMomentum final : public wbt::base::WBBlock +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + CentroidalMomentum(); + ~CentroidalMomentum() override; + + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; +}; + +#endif // WBT_CENTROIDALMOMENTUM_H diff --git a/toolbox/library/include/Block/DiscreteFilter.h b/toolbox/library/include/WBToolbox/Block/DiscreteFilter.h similarity index 68% rename from toolbox/library/include/Block/DiscreteFilter.h rename to toolbox/library/include/WBToolbox/Block/DiscreteFilter.h index c04a89531..d9f5f698c 100644 --- a/toolbox/library/include/Block/DiscreteFilter.h +++ b/toolbox/library/include/WBToolbox/Block/DiscreteFilter.h @@ -9,16 +9,23 @@ #ifndef WBT_FILTER_H #define WBT_FILTER_H -#include "Core/Block.h" +#include #include #include namespace wbt { - class BlockInformation; - class DiscreteFilter; + namespace block { + class DiscreteFilter; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + // clang-format off /** * @brief The wbt::DiscreteFilter class @@ -41,7 +48,7 @@ namespace wbt { * */ // clang-format on -class wbt::DiscreteFilter final : public wbt::Block +class wbt::block::DiscreteFilter final : public blockfactory::core::Block { private: class impl; @@ -52,11 +59,12 @@ class wbt::DiscreteFilter final : public wbt::Block ~DiscreteFilter() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool initializeInitialConditions(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool + initializeInitialConditions(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_FILTER_H diff --git a/toolbox/library/include/Block/DotJNu.h b/toolbox/library/include/WBToolbox/Block/DotJNu.h similarity index 53% rename from toolbox/library/include/Block/DotJNu.h rename to toolbox/library/include/WBToolbox/Block/DotJNu.h index f0e166ec5..d4c645812 100644 --- a/toolbox/library/include/Block/DotJNu.h +++ b/toolbox/library/include/WBToolbox/Block/DotJNu.h @@ -9,16 +9,23 @@ #ifndef WBT_DOTJNU_H #define WBT_DOTJNU_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class DotJNu; + namespace block { + class DotJNu; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::DotJNu class * @@ -31,7 +38,7 @@ namespace wbt { * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "Frame" | * */ -class wbt::DotJNu final : public wbt::WBBlock +class wbt::block::DotJNu final : public wbt::base::WBBlock { private: class impl; @@ -42,11 +49,11 @@ class wbt::DotJNu final : public wbt::WBBlock ~DotJNu() override; unsigned numberOfParameters() override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool parseParameters(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_DOTJNU_H diff --git a/toolbox/library/include/Block/ForwardKinematics.h b/toolbox/library/include/WBToolbox/Block/ForwardKinematics.h similarity index 55% rename from toolbox/library/include/Block/ForwardKinematics.h rename to toolbox/library/include/WBToolbox/Block/ForwardKinematics.h index 8a52a41dd..fb0f43fdb 100644 --- a/toolbox/library/include/Block/ForwardKinematics.h +++ b/toolbox/library/include/WBToolbox/Block/ForwardKinematics.h @@ -9,16 +9,23 @@ #ifndef WBT_FORWARDKINEMATICS_H #define WBT_FORWARDKINEMATICS_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class ForwardKinematics; + namespace block { + class ForwardKinematics; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::ForwardKinematics class * @@ -31,7 +38,7 @@ namespace wbt { * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "Frame" | * */ -class wbt::ForwardKinematics final : public wbt::WBBlock +class wbt::block::ForwardKinematics final : public wbt::base::WBBlock { private: class impl; @@ -42,11 +49,11 @@ class wbt::ForwardKinematics final : public wbt::WBBlock ~ForwardKinematics() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_FORWARDKINEMATICS_H diff --git a/toolbox/library/include/Block/GetLimits.h b/toolbox/library/include/WBToolbox/Block/GetLimits.h similarity index 50% rename from toolbox/library/include/Block/GetLimits.h rename to toolbox/library/include/WBToolbox/Block/GetLimits.h index 734d9a5b1..6cb7dff65 100644 --- a/toolbox/library/include/Block/GetLimits.h +++ b/toolbox/library/include/WBToolbox/Block/GetLimits.h @@ -9,16 +9,23 @@ #ifndef WBT_GETLIMITS_H #define WBT_GETLIMITS_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class GetLimits; + namespace block { + class GetLimits; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::GetLimits class * @@ -31,7 +38,7 @@ namespace wbt { * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "LimitType" | * */ -class wbt::GetLimits final : public wbt::WBBlock +class wbt::block::GetLimits final : public wbt::base::WBBlock { private: class impl; @@ -42,12 +49,13 @@ class wbt::GetLimits final : public wbt::WBBlock ~GetLimits() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool initializeInitialConditions(const BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool + initializeInitialConditions(const blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_GETLIMITS_H diff --git a/toolbox/library/include/Block/GetMeasurement.h b/toolbox/library/include/WBToolbox/Block/GetMeasurement.h similarity index 54% rename from toolbox/library/include/Block/GetMeasurement.h rename to toolbox/library/include/WBToolbox/Block/GetMeasurement.h index 6f271991e..26bb3c4ed 100644 --- a/toolbox/library/include/Block/GetMeasurement.h +++ b/toolbox/library/include/WBToolbox/Block/GetMeasurement.h @@ -9,16 +9,23 @@ #ifndef WBT_GETMEASUREMENT_H #define WBT_GETMEASUREMENT_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class GetMeasurement; + namespace block { + class GetMeasurement; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::GetMeasurement class * @@ -31,7 +38,7 @@ namespace wbt { * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "MeasuredType" | * */ -class wbt::GetMeasurement final : public wbt::WBBlock +class wbt::block::GetMeasurement final : public wbt::base::WBBlock { private: class impl; @@ -42,11 +49,11 @@ class wbt::GetMeasurement final : public wbt::WBBlock ~GetMeasurement() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_GETMEASUREMENT_H diff --git a/toolbox/library/include/WBToolbox/Block/InverseDynamics.h b/toolbox/library/include/WBToolbox/Block/InverseDynamics.h new file mode 100644 index 000000000..6f7247f99 --- /dev/null +++ b/toolbox/library/include/WBToolbox/Block/InverseDynamics.h @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef WBT_INVERSEDYNAMICS_H +#define WBT_INVERSEDYNAMICS_H + +#include "WBToolbox/Base/WBBlock.h" + +#include +#include + +namespace wbt { + namespace block { + class InverseDynamics; + } // namespace block +} // namespace wbt + +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + +/** + * @brief The wbt::InverseDynamics class + */ +class wbt::block::InverseDynamics final : public wbt::base::WBBlock +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + InverseDynamics(); + ~InverseDynamics() override; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; +}; + +#endif // WBT_INVERSEDYNAMICS_H diff --git a/toolbox/library/include/Block/InverseKinematics.h b/toolbox/library/include/WBToolbox/Block/InverseKinematics.h similarity index 74% rename from toolbox/library/include/Block/InverseKinematics.h rename to toolbox/library/include/WBToolbox/Block/InverseKinematics.h index 87a572280..97f621c9a 100644 --- a/toolbox/library/include/Block/InverseKinematics.h +++ b/toolbox/library/include/WBToolbox/Block/InverseKinematics.h @@ -18,9 +18,9 @@ class wbt::InverseKinematics : public wbt::WBIModelBlock InverseKinematics(); virtual unsigned numberOfParameters(); - virtual bool configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error); + virtual bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo, wbt::Error* error); - virtual bool initialize(BlockInformation* blockInfo, wbt::Error* error); + virtual bool initialize(blockfactory::core::BlockInformation* blockInfo, wbt::Error* error); virtual bool terminate(BlockInformation* blockInfo, wbt::Error* error); virtual bool output(BlockInformation* blockInfo, wbt::Error* error); }; diff --git a/toolbox/library/include/Block/Jacobian.h b/toolbox/library/include/WBToolbox/Block/Jacobian.h similarity index 53% rename from toolbox/library/include/Block/Jacobian.h rename to toolbox/library/include/WBToolbox/Block/Jacobian.h index d0704eaa4..65970be15 100644 --- a/toolbox/library/include/Block/Jacobian.h +++ b/toolbox/library/include/WBToolbox/Block/Jacobian.h @@ -9,16 +9,23 @@ #ifndef WBT_JACOBIAN_H #define WBT_JACOBIAN_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class Jacobian; + namespace block { + class Jacobian; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::Jacobian class * @@ -31,7 +38,7 @@ namespace wbt { * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "Frame" | * */ -class wbt::Jacobian final : public wbt::WBBlock +class wbt::block::Jacobian final : public wbt::base::WBBlock { private: class impl; @@ -42,11 +49,11 @@ class wbt::Jacobian final : public wbt::WBBlock ~Jacobian() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_JACOBIAN_H diff --git a/toolbox/library/include/WBToolbox/Block/MassMatrix.h b/toolbox/library/include/WBToolbox/Block/MassMatrix.h new file mode 100644 index 000000000..7988484ef --- /dev/null +++ b/toolbox/library/include/WBToolbox/Block/MassMatrix.h @@ -0,0 +1,48 @@ +/* + * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef WBT_MASSMATRIX_H +#define WBT_MASSMATRIX_H + +#include "WBToolbox/Base/WBBlock.h" + +#include +#include + +namespace wbt { + namespace block { + class MassMatrix; + } // namespace block +} // namespace wbt + +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + +/** + * @brief The wbt::MassMatrix class + */ +class wbt::block::MassMatrix final : public wbt::base::WBBlock +{ +private: + class impl; + std::unique_ptr pImpl; + +public: + MassMatrix(); + ~MassMatrix() override; + + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; +}; + +#endif // WBT_MASSMATRIX_H diff --git a/toolbox/library/include/Block/MinimumJerkTrajectoryGenerator.h b/toolbox/library/include/WBToolbox/Block/MinimumJerkTrajectoryGenerator.h similarity index 68% rename from toolbox/library/include/Block/MinimumJerkTrajectoryGenerator.h rename to toolbox/library/include/WBToolbox/Block/MinimumJerkTrajectoryGenerator.h index 1eb8f77ad..310d808e6 100644 --- a/toolbox/library/include/Block/MinimumJerkTrajectoryGenerator.h +++ b/toolbox/library/include/WBToolbox/Block/MinimumJerkTrajectoryGenerator.h @@ -9,16 +9,23 @@ #ifndef WBT_MINJERKTRAJGENERATOR_H #define WBT_MINJERKTRAJGENERATOR_H -#include "Core/Block.h" +#include #include #include namespace wbt { - class BlockInformation; - class MinimumJerkTrajectoryGenerator; + namespace block { + class MinimumJerkTrajectoryGenerator; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::MinimumJerkTrajectoryGenerator class * @@ -37,7 +44,7 @@ namespace wbt { * | ::BOOL | 6 + Block::NumberOfParameters | 1 | 1 | "ResetOnSettlingTimeChange" | * */ -class wbt::MinimumJerkTrajectoryGenerator final : public wbt::Block +class wbt::block::MinimumJerkTrajectoryGenerator final : public blockfactory::core::Block { private: class impl; @@ -48,10 +55,10 @@ class wbt::MinimumJerkTrajectoryGenerator final : public wbt::Block ~MinimumJerkTrajectoryGenerator() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_MINJERKTRAJGENERATOR_H diff --git a/toolbox/library/include/Block/QpOases.h b/toolbox/library/include/WBToolbox/Block/QpOases.h similarity index 63% rename from toolbox/library/include/Block/QpOases.h rename to toolbox/library/include/WBToolbox/Block/QpOases.h index b400db063..e0082e941 100644 --- a/toolbox/library/include/Block/QpOases.h +++ b/toolbox/library/include/WBToolbox/Block/QpOases.h @@ -6,19 +6,26 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "Core/Block.h" +#ifndef WBT_QPOASES_H +#define WBT_QPOASES_H + +#include #include #include -#ifndef WBT_QPOASES_H -#define WBT_QPOASES_H - namespace wbt { - class BlockInformation; - class QpOases; + namespace block { + class QpOases; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::QpOases class * @@ -36,7 +43,7 @@ namespace wbt { * | ParameterType::BOOL | 5 + Block::NumberOfParameters | 1 | 1 | "StopWhenFails" | * */ -class wbt::QpOases final : public wbt::Block +class wbt::block::QpOases final : public blockfactory::core::Block { private: class impl; @@ -47,11 +54,12 @@ class wbt::QpOases final : public wbt::Block ~QpOases() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool initializeInitialConditions(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool + initializeInitialConditions(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_QPOASES_H diff --git a/toolbox/library/include/Block/RealTimeSynchronizer.h b/toolbox/library/include/WBToolbox/Block/RealTimeSynchronizer.h similarity index 55% rename from toolbox/library/include/Block/RealTimeSynchronizer.h rename to toolbox/library/include/WBToolbox/Block/RealTimeSynchronizer.h index 0ca0bc1b8..373a14e66 100644 --- a/toolbox/library/include/Block/RealTimeSynchronizer.h +++ b/toolbox/library/include/WBToolbox/Block/RealTimeSynchronizer.h @@ -9,16 +9,23 @@ #ifndef WBT_REALTIMESYNCHRONIZER_H #define WBT_REALTIMESYNCHRONIZER_H -#include "Core/Block.h" +#include #include #include namespace wbt { - class BlockInformation; - class RealTimeSynchronizer; + namespace block { + class RealTimeSynchronizer; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::RealTimeSynchronizer class * @@ -31,7 +38,7 @@ namespace wbt { * | ::DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "Period" | * */ -class wbt::RealTimeSynchronizer final : public wbt::Block +class wbt::block::RealTimeSynchronizer final : public blockfactory::core::Block { private: class impl; @@ -42,11 +49,11 @@ class wbt::RealTimeSynchronizer final : public wbt::Block ~RealTimeSynchronizer() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_REALTIMESYNCHRONIZER_H diff --git a/toolbox/library/include/Block/RelativeTransform.h b/toolbox/library/include/WBToolbox/Block/RelativeTransform.h similarity index 56% rename from toolbox/library/include/Block/RelativeTransform.h rename to toolbox/library/include/WBToolbox/Block/RelativeTransform.h index 14e9fba5f..d67620b20 100644 --- a/toolbox/library/include/Block/RelativeTransform.h +++ b/toolbox/library/include/WBToolbox/Block/RelativeTransform.h @@ -9,16 +9,23 @@ #ifndef WBT_RELATIVETRASFORM_H #define WBT_RELATIVETRASFORM_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class RelativeTransform; + namespace block { + class RelativeTransform; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::RelativeTransform class * @@ -32,7 +39,7 @@ namespace wbt { * | ::STRING | 1 + WBBlock::NumberOfParameters | 1 | 1 | "Frame2" | * */ -class wbt::RelativeTransform final : public wbt::WBBlock +class wbt::block::RelativeTransform final : public wbt::base::WBBlock { private: class impl; @@ -43,11 +50,11 @@ class wbt::RelativeTransform final : public wbt::WBBlock ~RelativeTransform() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_RELATIVETRASFORM_H diff --git a/toolbox/library/include/Block/RemoteInverseKinematics.h b/toolbox/library/include/WBToolbox/Block/RemoteInverseKinematics.h similarity index 70% rename from toolbox/library/include/Block/RemoteInverseKinematics.h rename to toolbox/library/include/WBToolbox/Block/RemoteInverseKinematics.h index 9b6621fb8..3e927e614 100644 --- a/toolbox/library/include/Block/RemoteInverseKinematics.h +++ b/toolbox/library/include/WBToolbox/Block/RemoteInverseKinematics.h @@ -8,7 +8,7 @@ namespace wbt { class BlockInformation; } // namespace wbt -class wbt::RemoteInverseKinematics : public wbt::Block +class wbt::RemoteInverseKinematics : public blockfactory::core::Block { struct RemoteInverseKinematicsPimpl; @@ -20,9 +20,9 @@ class wbt::RemoteInverseKinematics : public wbt::Block virtual unsigned numberOfParameters(); virtual unsigned numberOfDiscreteStates(); - virtual bool configureSizeAndPorts(BlockInformation* blockInfo, wbt::Error* error); + virtual bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo, wbt::Error* error); - virtual bool initialize(BlockInformation* blockInfo, wbt::Error* error); + virtual bool initialize(blockfactory::core::BlockInformation* blockInfo, wbt::Error* error); virtual bool terminate(BlockInformation* blockInfo, wbt::Error* error); virtual bool output(BlockInformation* blockInfo, wbt::Error* error); }; diff --git a/toolbox/library/include/Block/SetMotorParameters.h b/toolbox/library/include/WBToolbox/Block/SetMotorParameters.h similarity index 66% rename from toolbox/library/include/Block/SetMotorParameters.h rename to toolbox/library/include/WBToolbox/Block/SetMotorParameters.h index a00718820..48eda23e6 100644 --- a/toolbox/library/include/Block/SetMotorParameters.h +++ b/toolbox/library/include/WBToolbox/Block/SetMotorParameters.h @@ -10,16 +10,23 @@ #ifndef WBT_SETMOTORPARAMETERS_H #define WBT_SETMOTORPARAMETERS_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class SetMotorParameters; + namespace block { + class SetMotorParameters; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::SetMotorParameters class * @@ -39,7 +46,7 @@ namespace wbt { * | ::STRING | 7 + WBBlock::NumberOfParameters | 1 | ParameterMetadata::DynamicSize | "Bemf" | * * */ -class wbt::SetMotorParameters final : public wbt::WBBlock +class wbt::block::SetMotorParameters final : public wbt::base::WBBlock { private: class impl; @@ -50,11 +57,11 @@ class wbt::SetMotorParameters final : public wbt::WBBlock ~SetMotorParameters() override; unsigned numberOfParameters() override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool parseParameters(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_SETMOTORPARAMETERS_H diff --git a/toolbox/library/include/Block/SetReferences.h b/toolbox/library/include/WBToolbox/Block/SetReferences.h similarity index 53% rename from toolbox/library/include/Block/SetReferences.h rename to toolbox/library/include/WBToolbox/Block/SetReferences.h index 3e6221588..5a2060e26 100644 --- a/toolbox/library/include/Block/SetReferences.h +++ b/toolbox/library/include/WBToolbox/Block/SetReferences.h @@ -9,16 +9,23 @@ #ifndef WBT_SETREFERENCES_H #define WBT_SETREFERENCES_H -#include "Base/WBBlock.h" +#include "WBToolbox/Base/WBBlock.h" #include #include namespace wbt { - class BlockInformation; - class SetReferences; + namespace block { + class SetReferences; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::SetReferences class * @@ -32,7 +39,7 @@ namespace wbt { * | ::DOUBLE | 0 + WBBlock::NumberOfParameters | 1 | 1 | "TrajectoryReference" | * */ -class wbt::SetReferences final : public wbt::WBBlock +class wbt::block::SetReferences final : public wbt::base::WBBlock { private: class impl; @@ -43,12 +50,13 @@ class wbt::SetReferences final : public wbt::WBBlock ~SetReferences() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool initializeInitialConditions(const BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool + initializeInitialConditions(const blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_SETREFERENCES_H diff --git a/toolbox/library/include/Block/SimulatorSynchronizer.h b/toolbox/library/include/WBToolbox/Block/SimulatorSynchronizer.h similarity index 60% rename from toolbox/library/include/Block/SimulatorSynchronizer.h rename to toolbox/library/include/WBToolbox/Block/SimulatorSynchronizer.h index a5ae32963..85fd6be8c 100644 --- a/toolbox/library/include/Block/SimulatorSynchronizer.h +++ b/toolbox/library/include/WBToolbox/Block/SimulatorSynchronizer.h @@ -9,17 +9,24 @@ #ifndef WBT_SIMULATORSYNCHRONIZER_H #define WBT_SIMULATORSYNCHRONIZER_H -#include "Core/Block.h" +#include #include #include #include namespace wbt { - class BlockInformation; - class SimulatorSynchronizer; + namespace block { + class SimulatorSynchronizer; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::SimulatorSynchronizer class * @@ -34,7 +41,7 @@ namespace wbt { * | ::STRING | 2 + WBBlock::NumberOfParameters | 1 | 1 | "GazeboClockPort" | * */ -class wbt::SimulatorSynchronizer final : public wbt::Block +class wbt::block::SimulatorSynchronizer final : public blockfactory::core::Block { private: class impl; @@ -46,11 +53,11 @@ class wbt::SimulatorSynchronizer final : public wbt::Block unsigned numberOfParameters() override; std::vector additionalBlockOptions() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_SIMULATORSYNCHRONIZER_H diff --git a/toolbox/library/include/WBToolbox/Block/YarpClock.h b/toolbox/library/include/WBToolbox/Block/YarpClock.h new file mode 100644 index 000000000..dc3c949ce --- /dev/null +++ b/toolbox/library/include/WBToolbox/Block/YarpClock.h @@ -0,0 +1,44 @@ +/* + * Copyright (C) 2018 Istituto Italiano di Tecnologia (IIT) + * All rights reserved. + * + * This software may be modified and distributed under the terms of the + * GNU Lesser General Public License v2.1 or any later version. + */ + +#ifndef WBT_YARPCLOCK_H +#define WBT_YARPCLOCK_H + +#include + +#include + +namespace wbt { + namespace block { + class YarpClock; + } // namespace block +} // namespace wbt + +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + +/** + * @brief The wbt::YarpClock class + */ +class wbt::block::YarpClock final : public blockfactory::core::Block +{ +public: + YarpClock() = default; + ~YarpClock() override = default; + + unsigned numberOfParameters() override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; +}; + +#endif // WBT_YARPCLOCK_H diff --git a/toolbox/library/include/Block/YarpRead.h b/toolbox/library/include/WBToolbox/Block/YarpRead.h similarity index 64% rename from toolbox/library/include/Block/YarpRead.h rename to toolbox/library/include/WBToolbox/Block/YarpRead.h index 9b164370c..83b91c8f8 100644 --- a/toolbox/library/include/Block/YarpRead.h +++ b/toolbox/library/include/WBToolbox/Block/YarpRead.h @@ -9,16 +9,23 @@ #ifndef WBT_YARPREAD_H #define WBT_YARPREAD_H -#include "Core/Block.h" +#include #include #include namespace wbt { - class BlockInformation; - class YarpRead; + namespace block { + class YarpRead; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::YarpRead class * @@ -35,7 +42,7 @@ namespace wbt { * | ::BOOL | 6 + Block::NumberOfParameters | 1 | 1 | "ErrorOnMissingPort" | * */ -class wbt::YarpRead final : public wbt::Block +class wbt::block::YarpRead final : public blockfactory::core::Block { private: class impl; @@ -46,11 +53,11 @@ class wbt::YarpRead final : public wbt::Block ~YarpRead() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_YARPREAD_H diff --git a/toolbox/library/include/Block/YarpWrite.h b/toolbox/library/include/WBToolbox/Block/YarpWrite.h similarity index 57% rename from toolbox/library/include/Block/YarpWrite.h rename to toolbox/library/include/WBToolbox/Block/YarpWrite.h index 1b7e74003..310d53880 100644 --- a/toolbox/library/include/Block/YarpWrite.h +++ b/toolbox/library/include/WBToolbox/Block/YarpWrite.h @@ -9,16 +9,23 @@ #ifndef WBT_YARPWRITE_H #define WBT_YARPWRITE_H -#include "Core/Block.h" +#include #include #include namespace wbt { - class BlockInformation; - class YarpWrite; + namespace block { + class YarpWrite; + } // namespace block } // namespace wbt +namespace blockfactory { + namespace core { + class BlockInformation; + } // namespace core +} // namespace blockfactory + /** * @brief The wbt::YarpWrite class * @@ -31,7 +38,7 @@ namespace wbt { * | ::BOOL | 2 + Block::NumberOfParameters | 1 | 1 | "ErrorOnMissingPort" | * */ -class wbt::YarpWrite final : public wbt::Block +class wbt::block::YarpWrite final : public blockfactory::core::Block { private: class impl; @@ -42,11 +49,11 @@ class wbt::YarpWrite final : public wbt::Block ~YarpWrite() override; unsigned numberOfParameters() override; - bool parseParameters(BlockInformation* blockInfo) override; - bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(BlockInformation* blockInfo) override; - bool terminate(const BlockInformation* blockInfo) override; - bool output(const BlockInformation* blockInfo) override; + bool parseParameters(blockfactory::core::BlockInformation* blockInfo) override; + bool configureSizeAndPorts(blockfactory::core::BlockInformation* blockInfo) override; + bool initialize(blockfactory::core::BlockInformation* blockInfo) override; + bool terminate(const blockfactory::core::BlockInformation* blockInfo) override; + bool output(const blockfactory::core::BlockInformation* blockInfo) override; }; #endif // WBT_YARPWRITE_H diff --git a/toolbox/library/src/CentroidalMomentum.cpp b/toolbox/library/src/CentroidalMomentum.cpp index 53f9e6c11..36bd0dcd3 100644 --- a/toolbox/library/src/CentroidalMomentum.cpp +++ b/toolbox/library/src/CentroidalMomentum.cpp @@ -6,13 +6,13 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "CentroidalMomentum.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/CentroidalMomentum.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" +#include +#include +#include #include #include #include @@ -20,7 +20,8 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -79,23 +80,21 @@ bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Vector representing the centroidal momentum (1x6) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple(InputIndex::BasePose, std::vector{4, 4}, DataType::DOUBLE), - std::make_tuple( - InputIndex::JointConfiguration, std::vector{dofs}, DataType::DOUBLE), - std::make_tuple(InputIndex::BaseVelocity, std::vector{6}, DataType::DOUBLE), - std::make_tuple(InputIndex::JointVelocity, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::BasePose, Port::Dimensions{4, 4}, Port::Port::DataType::DOUBLE}, + {InputIndex::JointConfiguration, Port::Dimensions{dofs}, Port::Port::DataType::DOUBLE}, + {InputIndex::BaseVelocity, Port::Dimensions{6}, Port::Port::DataType::DOUBLE}, + {InputIndex::JointVelocity, Port::Dimensions{dofs}, Port::Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple(OutputIndex::CentroidalMomentum, std::vector{6}, DataType::DOUBLE), - }, - }); + {OutputIndex::CentroidalMomentum, Port::Dimensions{6}, Port::Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -117,7 +116,7 @@ bool CentroidalMomentum::output(const BlockInformation* blockInfo) // Get the KinDynComputations object auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -130,7 +129,7 @@ bool CentroidalMomentum::output(const BlockInformation* blockInfo) InputSignalPtr jointsVelocitySignal = blockInfo->getInputPortSignal(InputIndex::JointVelocity); if (!basePoseSig || !jointsPosSig || !baseVelocitySignal || !jointsVelocitySignal) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } @@ -138,7 +137,7 @@ bool CentroidalMomentum::output(const BlockInformation* blockInfo) basePoseSig, jointsPosSig, baseVelocitySignal, jointsVelocitySignal, kinDyn.get()); if (!ok) { - wbtError << "Failed to set the robot state."; + bfError << "Failed to set the robot state."; return false; } @@ -151,13 +150,13 @@ bool CentroidalMomentum::output(const BlockInformation* blockInfo) // Get the output signal OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::CentroidalMomentum); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } // Fill the output buffer if (!output->setBuffer(toEigen(pImpl->centroidalMomentum).data(), output->getWidth())) { - wbtError << "Failed to set output buffer."; + bfError << "Failed to set output buffer."; return false; } diff --git a/toolbox/library/src/DiscreteFilter.cpp b/toolbox/library/src/DiscreteFilter.cpp index 6785c59aa..804a5c131 100644 --- a/toolbox/library/src/DiscreteFilter.cpp +++ b/toolbox/library/src/DiscreteFilter.cpp @@ -6,13 +6,13 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "DiscreteFilter.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/DiscreteFilter.h" +#include +#include +#include +#include +#include #include #include @@ -21,7 +21,8 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; using namespace iCub::ctrl; using namespace yarp::sig; @@ -87,7 +88,7 @@ bool DiscreteFilter::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -108,22 +109,22 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo) // 1) The output signal (1xn) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple( - InputIndex::InputSignal, std::vector{Signal::DynamicSize}, DataType::DOUBLE), + {InputIndex::InputSignal, + Port::Dimensions{Port::DynamicSize}, + Port::Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple(OutputIndex::FilteredSignal, - std::vector{Signal::DynamicSize}, - DataType::DOUBLE), - }, - }); + {OutputIndex::FilteredSignal, + Port::Dimensions{Port::DynamicSize}, + Port::Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -140,7 +141,7 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo) // ========== if (!DiscreteFilter::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -166,7 +167,7 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("MedianOrder", medianFilter_order); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -176,13 +177,13 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo) if (filter_type == "Generic") { // Check if numerator and denominator are not empty if (num_coeff.empty() || den_coeff.empty()) { - wbtError << "Empty numerator or denominator not allowed."; + bfError << "Empty numerator or denominator not allowed."; return false; } // Check if numerator or denominator are scalar and zero if ((num_coeff.size() == 1 && num_coeff.front() == 0.0) || (den_coeff.size() == 1 && den_coeff.front() == 0.0)) { - wbtError << "Passed numerator or denominator not valid."; + bfError << "Passed numerator or denominator not valid."; return false; } } @@ -198,12 +199,12 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo) if (initStatus) { // y0 and output signal dimensions should match if (y0.size() != outputSignalSize) { - wbtError << "y0 and output signal sizes don't match."; + bfError << "y0 and output signal sizes don't match."; return false; } // u0 and input signal dimensions should match (used only for Generic) if ((filter_type == "Generic") && (u0.size() != inputPortWidth)) { - wbtError << "(Generic) u0 and input signal sizes don't match."; + bfError << "(Generic) u0 and input signal sizes don't match."; return false; } // Allocate the initial conditions @@ -230,8 +231,8 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo) // ----------------------- else if (filter_type == "FirstOrderLowPassFilter") { if (firstOrderLowPassFilter_fc == 0.0 || firstOrderLowPassFilter_ts == 0.0) { - wbtError << "(FirstOrderLowPassFilter) You need to " - "specify Fc and Ts."; + bfError << "(FirstOrderLowPassFilter) You need to " + "specify Fc and Ts."; return false; } pImpl->filter = std::unique_ptr( @@ -241,13 +242,13 @@ bool DiscreteFilter::initialize(BlockInformation* blockInfo) // ------------ else if (filter_type == "MedianFilter") { if (medianFilter_order == 0) { - wbtError << "(MedianFilter) You need to specify the filter order."; + bfError << "(MedianFilter) You need to specify the filter order."; return false; } pImpl->filter = std::unique_ptr(new MedianFilter(medianFilter_order)); } else { - wbtError << "Filter type not recognized."; + bfError << "Filter type not recognized."; return false; } @@ -281,7 +282,7 @@ bool DiscreteFilter::initializeInitialConditions(const BlockInformation* /*block pImpl->filter->init(pImpl->y0); } else { - wbtError << "Failed to get the IFilter object."; + bfError << "Failed to get the IFilter object."; return false; } } @@ -292,7 +293,7 @@ bool DiscreteFilter::initializeInitialConditions(const BlockInformation* /*block bool DiscreteFilter::output(const BlockInformation* blockInfo) { if (!pImpl->filter) { - wbtError << "Failed to retrieve the filter object."; + bfError << "Failed to retrieve the filter object."; return false; } @@ -301,7 +302,7 @@ bool DiscreteFilter::output(const BlockInformation* blockInfo) OutputSignalPtr outputSignal = blockInfo->getOutputPortSignal(OutputIndex::FilteredSignal); if (!inputSignal || !outputSignal) { - wbtError << "Signals not valid."; + bfError << "Signals not valid."; return false; } @@ -315,7 +316,7 @@ bool DiscreteFilter::output(const BlockInformation* blockInfo) // Forward the filtered signals to the output port if (!outputSignal->setBuffer(outputVector.data(), outputVector.length())) { - wbtError << "Failed to set output buffer."; + bfError << "Failed to set output buffer."; return false; } diff --git a/toolbox/library/src/DotJNu.cpp b/toolbox/library/src/DotJNu.cpp index 7b0a1b4ea..1a4980c41 100644 --- a/toolbox/library/src/DotJNu.cpp +++ b/toolbox/library/src/DotJNu.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "DotJNu.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/DotJNu.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -24,14 +24,15 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, Frame }; @@ -78,7 +79,7 @@ bool DotJNu::parseParameters(BlockInformation* blockInfo) const ParameterMetadata frameMetadata(ParameterType::STRING, ParamIndex::Frame, 1, 1, "Frame"); if (!blockInfo->addParameterMetadata(frameMetadata)) { - wbtError << "Failed to store parameters metadata."; + bfError << "Failed to store parameters metadata."; return false; } @@ -108,23 +109,21 @@ bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Vector representing the \dot{J} \nu vector (1x6) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple(InputIndex::BasePose, std::vector{4, 4}, DataType::DOUBLE), - std::make_tuple( - InputIndex::JointConfiguration, std::vector{dofs}, DataType::DOUBLE), - std::make_tuple(InputIndex::BaseVelocity, std::vector{6}, DataType::DOUBLE), - std::make_tuple(InputIndex::JointVelocity, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::BasePose, Port::Dimensions{4, 4}, Port::DataType::DOUBLE}, + {InputIndex::JointConfiguration, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, + {InputIndex::BaseVelocity, Port::Dimensions{6}, Port::DataType::DOUBLE}, + {InputIndex::JointVelocity, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple(OutputIndex::DotJNu, std::vector{6}, DataType::DOUBLE), - }, - }); + {OutputIndex::DotJNu, Port::Dimensions{6}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -141,13 +140,13 @@ bool DotJNu::initialize(BlockInformation* blockInfo) // ========== if (!DotJNu::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } std::string frame; if (!m_parameters.getParameter("Frame", frame)) { - wbtError << "Cannot retrieve string from frame parameter."; + bfError << "Cannot retrieve string from frame parameter."; return false; } @@ -159,14 +158,14 @@ bool DotJNu::initialize(BlockInformation* blockInfo) auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Cannot retrieve handle to KinDynComputations."; + bfError << "Cannot retrieve handle to KinDynComputations."; return false; } if (frame != "com") { pImpl->frameIndex = kinDyn->getFrameIndex(frame); if (pImpl->frameIndex == iDynTree::FRAME_INVALID_INDEX) { - wbtError << "Cannot find " + frame + " in the frame list."; + bfError << "Cannot find " + frame + " in the frame list."; return false; } } @@ -192,7 +191,7 @@ bool DotJNu::output(const BlockInformation* blockInfo) // Get the KinDynComputations object auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -205,7 +204,7 @@ bool DotJNu::output(const BlockInformation* blockInfo) InputSignalPtr jointsVelocitySignal = blockInfo->getInputPortSignal(InputIndex::JointVelocity); if (!basePoseSig || !jointsPosSig || !baseVelocitySignal || !jointsVelocitySignal) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } @@ -213,7 +212,7 @@ bool DotJNu::output(const BlockInformation* blockInfo) basePoseSig, jointsPosSig, baseVelocitySignal, jointsVelocitySignal, kinDyn.get()); if (!ok) { - wbtError << "Failed to set the robot state."; + bfError << "Failed to set the robot state."; return false; } @@ -232,12 +231,12 @@ bool DotJNu::output(const BlockInformation* blockInfo) // Forward the output to Simulink OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::DotJNu); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } if (!output->setBuffer(pImpl->dotJNu.data(), output->getWidth())) { - wbtError << "Failed to set output buffer."; + bfError << "Failed to set output buffer."; } return true; diff --git a/toolbox/library/src/Factory.cpp b/toolbox/library/src/Factory.cpp index 3d2e7cf50..9f68a31bd 100644 --- a/toolbox/library/src/Factory.cpp +++ b/toolbox/library/src/Factory.cpp @@ -7,35 +7,35 @@ */ // YARP-dependent blocks -#include "Block/GetLimits.h" -#include "Block/GetMeasurement.h" -#include "Block/RealTimeSynchronizer.h" -#include "Block/SetMotorParameters.h" -#include "Block/SetReferences.h" -#include "Block/SimulatorSynchronizer.h" -#include "Block/YarpClock.h" -#include "Block/YarpRead.h" -#include "Block/YarpWrite.h" +#include "WBToolbox/Block/GetLimits.h" +#include "WBToolbox/Block/GetMeasurement.h" +#include "WBToolbox/Block/RealTimeSynchronizer.h" +#include "WBToolbox/Block/SetMotorParameters.h" +#include "WBToolbox/Block/SetReferences.h" +#include "WBToolbox/Block/SimulatorSynchronizer.h" +#include "WBToolbox/Block/YarpClock.h" +#include "WBToolbox/Block/YarpRead.h" +#include "WBToolbox/Block/YarpWrite.h" // #include "Block/RemoteInverseKinematics.h" // iDyntree-dependent blocks -#include "Block/CentroidalMomentum.h" -#include "Block/DotJNu.h" -#include "Block/ForwardKinematics.h" -#include "Block/InverseDynamics.h" -#include "Block/Jacobian.h" -#include "Block/MassMatrix.h" -#include "Block/RelativeTransform.h" +#include "WBToolbox/Block/CentroidalMomentum.h" +#include "WBToolbox/Block/DotJNu.h" +#include "WBToolbox/Block/ForwardKinematics.h" +#include "WBToolbox/Block/InverseDynamics.h" +#include "WBToolbox/Block/Jacobian.h" +#include "WBToolbox/Block/MassMatrix.h" +#include "WBToolbox/Block/RelativeTransform.h" // iCub-dependent blocks #ifdef WBT_USES_ICUB -#include "Block/DiscreteFilter.h" -#include "Block/MinimumJerkTrajectoryGenerator.h" +#include "WBToolbox/Block/DiscreteFilter.h" +#include "WBToolbox/Block/MinimumJerkTrajectoryGenerator.h" #endif // Other blocks #ifdef WBT_USES_QPOASES -#include "Block/QpOases.h" +#include "WBToolbox/Block/QpOases.h" #endif #ifdef WBT_USES_IPOPT // #include "Block/InverseKinematics.h" @@ -47,37 +47,57 @@ #include "shlibpp/SharedLibraryClassApi.h" // YARP-dependent blocks -SHLIBPP_DEFINE_SHARED_SUBCLASS(GetLimits, wbt::GetLimits, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(GetMeasurement, wbt::GetMeasurement, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(RealTimeSynchronizer, wbt::RealTimeSynchronizer, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(SetMotorParameters, wbt::SetMotorParameters, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(SetReferences, wbt::SetReferences, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(SimulatorSynchronizer, wbt::SimulatorSynchronizer, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(YarpClock, wbt::YarpClock, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(YarpRead, wbt::YarpRead, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(YarpWrite, wbt::YarpWrite, wbt::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(GetLimits, wbt::block::GetLimits, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(GetMeasurement, + wbt::block::GetMeasurement, + blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(RealTimeSynchronizer, + wbt::block::RealTimeSynchronizer, + blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(SetMotorParameters, + wbt::block::SetMotorParameters, + blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(SetReferences, wbt::block::SetReferences, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(SimulatorSynchronizer, + wbt::block::SimulatorSynchronizer, + blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(YarpClock, wbt::block::YarpClock, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(YarpRead, wbt::block::YarpRead, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(YarpWrite, wbt::block::YarpWrite, blockfactory::core::Block) // iDyntree-dependent blocks -SHLIBPP_DEFINE_SHARED_SUBCLASS(CentroidalMomentum, wbt::CentroidalMomentum, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(DotJNu, wbt::DotJNu, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(ForwardKinematics, wbt::ForwardKinematics, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(InverseDynamics, wbt::InverseDynamics, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(Jacobian, wbt::Jacobian, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(MassMatrix, wbt::MassMatrix, wbt::Block) -SHLIBPP_DEFINE_SHARED_SUBCLASS(RelativeTransform, wbt::RelativeTransform, wbt::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(CentroidalMomentum, + wbt::block::CentroidalMomentum, + blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(DotJNu, wbt::block::DotJNu, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(ForwardKinematics, + wbt::block::ForwardKinematics, + blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(InverseDynamics, + wbt::block::InverseDynamics, + blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(Jacobian, wbt::block::Jacobian, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(MassMatrix, wbt::block::MassMatrix, blockfactory::core::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(RelativeTransform, + wbt::block::RelativeTransform, + blockfactory::core::Block) // iCub-dependent blocks #ifdef WBT_USES_ICUB -SHLIBPP_DEFINE_SHARED_SUBCLASS(DiscreteFilter, wbt::DiscreteFilter, wbt::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(DiscreteFilter, + wbt::block::DiscreteFilter, + blockfactory::core::Block) SHLIBPP_DEFINE_SHARED_SUBCLASS(MinimumJerkTrajectoryGenerator, - wbt::MinimumJerkTrajectoryGenerator, - wbt::Block) + wbt::block::MinimumJerkTrajectoryGenerator, + blockfactory::core::Block) #endif // Other blocks #ifdef WBT_USES_QPOASES -SHLIBPP_DEFINE_SHARED_SUBCLASS(QpOases, wbt::QpOases, wbt::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(QpOases, wbt::block::QpOases, blockfactory::core::Block) #endif #ifdef WBT_USES_IPOPT -SHLIBPP_DEFINE_SHARED_SUBCLASS(InverseKinematics, wbt::InverseKinematics, wbt::Block) +SHLIBPP_DEFINE_SHARED_SUBCLASS(InverseKinematics, + wbt::block::InverseKinematics, + blockfactory::core::Block) #endif diff --git a/toolbox/library/src/ForwardKinematics.cpp b/toolbox/library/src/ForwardKinematics.cpp index 2392a2e05..fcdce0fdb 100644 --- a/toolbox/library/src/ForwardKinematics.cpp +++ b/toolbox/library/src/ForwardKinematics.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "ForwardKinematics.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/ForwardKinematics.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -24,14 +24,15 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, Frame }; @@ -75,7 +76,7 @@ bool ForwardKinematics::parseParameters(BlockInformation* blockInfo) const ParameterMetadata frameMetadata(ParameterType::STRING, ParamIndex::Frame, 1, 1, "Frame"); if (!blockInfo->addParameterMetadata(frameMetadata)) { - wbtError << "Failed to store parameters metadata."; + bfError << "Failed to store parameters metadata."; return false; } @@ -103,21 +104,19 @@ bool ForwardKinematics::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Homogeneous transformation between the world and the specified frame (4x4 matrix) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple(InputIndex::BasePose, std::vector{4, 4}, DataType::DOUBLE), - std::make_tuple( - InputIndex::JointConfiguration, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::BasePose, Port::Dimensions{4, 4}, Port::DataType::DOUBLE}, + {InputIndex::JointConfiguration, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple(OutputIndex::Transform, std::vector{4, 4}, DataType::DOUBLE), - }, - }); + {OutputIndex::Transform, Port::Dimensions{4, 4}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -134,13 +133,13 @@ bool ForwardKinematics::initialize(BlockInformation* blockInfo) // ========== if (!ForwardKinematics::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } std::string frame; if (!m_parameters.getParameter("Frame", frame)) { - wbtError << "Cannot retrieve string from frame parameter."; + bfError << "Cannot retrieve string from frame parameter."; return false; } @@ -152,14 +151,14 @@ bool ForwardKinematics::initialize(BlockInformation* blockInfo) auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Cannot retrieve handle to KinDynComputations."; + bfError << "Cannot retrieve handle to KinDynComputations."; return false; } if (frame != "com") { pImpl->frameIndex = kinDyn->getFrameIndex(frame); if (pImpl->frameIndex == iDynTree::FRAME_INVALID_INDEX) { - wbtError << "Cannot find " + frame + " in the frame list."; + bfError << "Cannot find " + frame + " in the frame list."; return false; } } @@ -185,7 +184,7 @@ bool ForwardKinematics::output(const BlockInformation* blockInfo) // Get the KinDynComputations object auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -196,14 +195,14 @@ bool ForwardKinematics::output(const BlockInformation* blockInfo) InputSignalPtr jointsPosSig = blockInfo->getInputPortSignal(InputIndex::JointConfiguration); if (!basePoseSig || !jointsPosSig) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } bool ok = setRobotState(basePoseSig, jointsPosSig, nullptr, nullptr, kinDyn.get()); if (!ok) { - wbtError << "Failed to set the robot state."; + bfError << "Failed to set the robot state."; return false; } @@ -223,7 +222,7 @@ bool ForwardKinematics::output(const BlockInformation* blockInfo) // Get the output signal memory location OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::Transform); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } diff --git a/toolbox/library/src/GetLimits.cpp b/toolbox/library/src/GetLimits.cpp index 4a37c9bc8..72a26ff68 100644 --- a/toolbox/library/src/GetLimits.cpp +++ b/toolbox/library/src/GetLimits.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "GetLimits.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/GetLimits.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -27,14 +27,15 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, LimitType }; @@ -68,7 +69,7 @@ class GetLimits::impl // BLOCK CLASS // =========== -wbt::GetLimits::GetLimits() +GetLimits::GetLimits() : pImpl{new impl()} {} @@ -85,7 +86,7 @@ bool GetLimits::parseParameters(BlockInformation* blockInfo) ParameterType::STRING, ParamIndex::LimitType, 1, 1, "LimitType"); if (!blockInfo->addParameterMetadata(limitTypeMetadata)) { - wbtError << "Failed to store parameters metadata."; + bfError << "Failed to store parameters metadata."; return false; } @@ -113,19 +114,18 @@ bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) // 2) Vector with the max limit (1xDoFs) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs }, { // Outputs - std::make_tuple(OutputIndex::MinLimit, std::vector{dofs}, DataType::DOUBLE), - std::make_tuple(OutputIndex::MaxLimit, std::vector{dofs}, DataType::DOUBLE), - }, - }); + {OutputIndex::MinLimit, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, + {OutputIndex::MaxLimit, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -144,13 +144,13 @@ bool GetLimits::initialize(BlockInformation* blockInfo) // ========== if (!GetLimits::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } // Read the control type if (!m_parameters.getParameter("LimitType", pImpl->limitType)) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -206,7 +206,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) || pImpl->limitType == "ControlBoardVelocity") { // Get the interface if (!robotInterface->getInterface(iControlLimits2) || !iControlLimits2) { - wbtError << "Failed to get IControlLimits2 interface."; + bfError << "Failed to get IControlLimits2 interface."; return false; } } @@ -214,7 +214,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) if (pImpl->limitType == "ControlBoardPosition") { for (unsigned i = 0; i < dofs; ++i) { if (!iControlLimits2->getLimits(i, &min, &max)) { - wbtError << "Failed to get limits from the interface."; + bfError << "Failed to get limits from the interface."; return false; } pImpl->limits.min[i] = GetLimits::impl::deg2rad(min); @@ -224,7 +224,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) else if (pImpl->limitType == "ControlBoardVelocity") { for (unsigned i = 0; i < dofs; ++i) { if (!iControlLimits2->getVelLimits(i, &min, &max)) { - wbtError << "Failed to get limits from the interface."; + bfError << "Failed to get limits from the interface."; return false; } pImpl->limits.min[i] = GetLimits::impl::deg2rad(min); @@ -243,7 +243,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) // Get the KinDynComputations pointer const auto& kindyncomp = robotInterface->getKinDynComputations(); if (!kindyncomp) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -259,7 +259,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) iDynTree::JointIndex jointIndex = model.getJointIndex(joint); if (jointIndex == iDynTree::JOINT_INVALID_INDEX) { - wbtError << "Invalid iDynTree joint index."; + bfError << "Invalid iDynTree joint index."; return false; } @@ -267,14 +267,14 @@ bool GetLimits::output(const BlockInformation* blockInfo) p_joint = model.getJoint(jointIndex); if (!p_joint->hasPosLimits()) { - wbtWarning << "Joint " << joint << " has no model limits."; + bfWarning << "Joint " << joint << " has no model limits."; pImpl->limits.min[i] = -std::numeric_limits::infinity(); pImpl->limits.max[i] = std::numeric_limits::infinity(); } else { if (!p_joint->getPosLimits(0, min, max)) { - wbtError << "Failed to get joint limits from the URDF model " - << "for the joint " << joint + "."; + bfError << "Failed to get joint limits from the URDF model " + << "for the joint " << joint + "."; return false; } pImpl->limits.min[i] = min; @@ -288,7 +288,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) // else if (limitType == "ModelEffort") { // } else { - wbtError << "Limit type " + pImpl->limitType + " not recognized."; + bfError << "Limit type " + pImpl->limitType + " not recognized."; return false; } } @@ -297,7 +297,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) OutputSignalPtr maxPort = blockInfo->getOutputPortSignal(OutputIndex::MaxLimit); if (!minPort || !maxPort) { - wbtError << "Output signals not valid."; + bfError << "Output signals not valid."; return false; } @@ -309,7 +309,7 @@ bool GetLimits::output(const BlockInformation* blockInfo) ok = ok && maxPort->setBuffer(pImpl->limits.max.data(), dofs); if (!ok) { - wbtError << "Failed to set output buffers."; + bfError << "Failed to set output buffers."; return false; } diff --git a/toolbox/library/src/GetMeasurement.cpp b/toolbox/library/src/GetMeasurement.cpp index e6726f403..850cd2dd5 100644 --- a/toolbox/library/src/GetMeasurement.cpp +++ b/toolbox/library/src/GetMeasurement.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "GetMeasurement.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/GetMeasurement.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -26,14 +26,15 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, MeasType }; @@ -95,7 +96,7 @@ bool GetMeasurement::parseParameters(BlockInformation* blockInfo) ParameterType::STRING, ParamIndex::MeasType, 1, 1, "MeasuredType"); if (!blockInfo->addParameterMetadata(measTypeMetadata)) { - wbtError << "Failed to store parameters metadata."; + bfError << "Failed to store parameters metadata."; return false; } @@ -122,18 +123,17 @@ bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Vector with the information asked (1xDoFs) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs }, { // Outputs - std::make_tuple(OutputIndex::Measurement, std::vector{dofs}, DataType::DOUBLE), - }, - }); + {OutputIndex::Measurement, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -150,14 +150,14 @@ bool GetMeasurement::initialize(BlockInformation* blockInfo) // ========== if (!GetMeasurement::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } // Read the measured type std::string measuredType; if (!m_parameters.getParameter("MeasuredType", measuredType)) { - wbtError << "Could not read measured type parameter."; + bfError << "Could not read measured type parameter."; return false; } @@ -190,7 +190,7 @@ bool GetMeasurement::initialize(BlockInformation* blockInfo) pImpl->measuredType = impl::MeasuredType::MOTOR_PWM; } else { - wbtError << "Measurement not supported."; + bfError << "Measurement not supported."; return false; } @@ -226,7 +226,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; if (!robotInterface->getInterface(iEncoders) || !iEncoders) { - wbtError << "Failed to get IEncoders interface."; + bfError << "Failed to get IEncoders interface."; return false; } // Get the measurement @@ -238,7 +238,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; if (!robotInterface->getInterface(iEncoders) || !iEncoders) { - wbtError << "Failed to get IEncoders interface."; + bfError << "Failed to get IEncoders interface."; return false; } // Get the measurement @@ -250,7 +250,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; if (!robotInterface->getInterface(iEncoders) || !iEncoders) { - wbtError << "Failed to get IEncoders interface."; + bfError << "Failed to get IEncoders interface."; return false; } // Get the measurement @@ -262,7 +262,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::ITorqueControl* iTorqueControl = nullptr; if (!robotInterface->getInterface(iTorqueControl) || !iTorqueControl) { - wbtError << "Failed to get ITorqueControl interface."; + bfError << "Failed to get ITorqueControl interface."; return false; } // Get the measurement @@ -277,7 +277,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IMotorEncoders* iMotorEncoders = nullptr; if (!robotInterface->getInterface(iMotorEncoders) || !iMotorEncoders) { - wbtError << "Failed to get IMotorEncoders interface."; + bfError << "Failed to get IMotorEncoders interface."; return false; } // Get the measurement @@ -289,7 +289,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IMotorEncoders* iMotorEncoders = nullptr; if (!robotInterface->getInterface(iMotorEncoders) || !iMotorEncoders) { - wbtError << "Failed to get IMotorEncoders interface."; + bfError << "Failed to get IMotorEncoders interface."; return false; } // Get the measurement @@ -301,7 +301,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IMotorEncoders* iMotorEncoders = nullptr; if (!robotInterface->getInterface(iMotorEncoders) || !iMotorEncoders) { - wbtError << "Failed to get IMotorEncoders interface."; + bfError << "Failed to get IMotorEncoders interface."; return false; } // Get the measurement @@ -313,7 +313,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::ICurrentControl* iCurrentControl = nullptr; if (!robotInterface->getInterface(iCurrentControl) || !iCurrentControl) { - wbtError << "Failed to get ICurrentControl interface."; + bfError << "Failed to get ICurrentControl interface."; return false; } // Get the measurement @@ -324,7 +324,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IPWMControl* iPWMControl = nullptr; if (!robotInterface->getInterface(iPWMControl) || !iPWMControl) { - wbtError << "Failed to get IPWMControl interface."; + bfError << "Failed to get IPWMControl interface."; return false; } // Get the measurement @@ -334,20 +334,20 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) } if (!ok) { - wbtError << "Failed to get measurement."; + bfError << "Failed to get measurement."; return false; } // Get the output signal OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::Measurement); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } // Fill the output buffer if (!output->setBuffer(pImpl->measurement.data(), output->getWidth())) { - wbtError << "Failed to set output buffer."; + bfError << "Failed to set output buffer."; return false; } diff --git a/toolbox/library/src/InverseDynamics.cpp b/toolbox/library/src/InverseDynamics.cpp index 645f7bd3a..2d2cc80c4 100644 --- a/toolbox/library/src/InverseDynamics.cpp +++ b/toolbox/library/src/InverseDynamics.cpp @@ -6,13 +6,13 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "InverseDynamics.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/InverseDynamics.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" +#include +#include +#include #include #include #include @@ -27,7 +27,8 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -97,26 +98,23 @@ bool InverseDynamics::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Vector representing the torques (1x(DoFs+6)) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple(InputIndex::BasePose, std::vector{4, 4}, DataType::DOUBLE), - std::make_tuple( - InputIndex::JointConfiguration, std::vector{dofs}, DataType::DOUBLE), - std::make_tuple(InputIndex::BaseVelocity, std::vector{6}, DataType::DOUBLE), - std::make_tuple(InputIndex::JointVelocity, std::vector{dofs}, DataType::DOUBLE), - std::make_tuple(InputIndex::BaseAcceleration, std::vector{6}, DataType::DOUBLE), - std::make_tuple( - InputIndex::JointAcceleration, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::BasePose, Port::Dimensions{4, 4}, Port::DataType::DOUBLE}, + {InputIndex::JointConfiguration, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, + {InputIndex::BaseVelocity, Port::Dimensions{6}, Port::DataType::DOUBLE}, + {InputIndex::JointVelocity, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, + {InputIndex::BaseAcceleration, Port::Dimensions{6}, Port::DataType::DOUBLE}, + {InputIndex::JointAcceleration, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple(OutputIndex::Torques, std::vector{dofs + 6}, DataType::DOUBLE), - }, - }); + {OutputIndex::Torques, Port::Dimensions{dofs + 6}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -145,7 +143,7 @@ bool InverseDynamics::initialize(BlockInformation* blockInfo) // Get the KinDynComputations pointer const auto& kindyn = getRobotInterface()->getKinDynComputations(); if (!kindyn) { - wbtError << "Failed to get the KinDynComputations object"; + bfError << "Failed to get the KinDynComputations object"; return false; } @@ -168,7 +166,7 @@ bool InverseDynamics::output(const BlockInformation* blockInfo) // Get the KinDynComputations object auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -181,7 +179,7 @@ bool InverseDynamics::output(const BlockInformation* blockInfo) InputSignalPtr jointsVelocitySignal = blockInfo->getInputPortSignal(InputIndex::JointVelocity); if (!basePoseSig || !jointsPosSig || !baseVelocitySignal || !jointsVelocitySignal) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } @@ -189,7 +187,7 @@ bool InverseDynamics::output(const BlockInformation* blockInfo) basePoseSig, jointsPosSig, baseVelocitySignal, jointsVelocitySignal, kinDyn.get()); if (!ok) { - wbtError << "Failed to set the robot state."; + bfError << "Failed to set the robot state."; return false; } @@ -199,14 +197,14 @@ bool InverseDynamics::output(const BlockInformation* blockInfo) InputSignalPtr baseAccelerationSignal = blockInfo->getInputPortSignal(InputIndex::BaseAcceleration); if (!baseAccelerationSignal) { - wbtError << "Base Acceleration signal not valid."; + bfError << "Base Acceleration signal not valid."; return false; } const double* bufBaseAcc = baseAccelerationSignal->getBuffer(); for (unsigned i = 0; i < baseAccelerationSignal->getWidth(); ++i) { if (!pImpl->baseAcceleration.setVal(i, bufBaseAcc[i])) { - wbtError << "Failed to fill base accelerations class member."; + bfError << "Failed to fill base accelerations class member."; return false; } } @@ -217,14 +215,14 @@ bool InverseDynamics::output(const BlockInformation* blockInfo) InputSignalPtr jointsAccelerationSignal = blockInfo->getInputPortSignal(InputIndex::JointAcceleration); if (!jointsAccelerationSignal) { - wbtError << "Joints Acceleration signal not valid."; + bfError << "Joints Acceleration signal not valid."; return false; } const double* bufJointsAcc = jointsAccelerationSignal->getBuffer(); for (unsigned i = 0; i < jointsAccelerationSignal->getWidth(); ++i) { if (!pImpl->jointsAcceleration.setVal(i, bufJointsAcc[i])) { - wbtError << "Failed to fill joint accelerations class member."; + bfError << "Failed to fill joint accelerations class member."; return false; } } @@ -239,14 +237,14 @@ bool InverseDynamics::output(const BlockInformation* blockInfo) pImpl->torques); if (!ok) { - wbtError << "iDynTree failed to compute inverse dynamics."; + bfError << "iDynTree failed to compute inverse dynamics."; return false; } // Get the output signal OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::Torques); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } double* outputBuffer = output->getBuffer(); diff --git a/toolbox/library/src/Jacobian.cpp b/toolbox/library/src/Jacobian.cpp index 198522fe6..d9526e8af 100644 --- a/toolbox/library/src/Jacobian.cpp +++ b/toolbox/library/src/Jacobian.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "Jacobian.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/Jacobian.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -26,14 +26,15 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, Frame }; @@ -82,7 +83,7 @@ bool Jacobian::parseParameters(BlockInformation* blockInfo) bool ok = blockInfo->addParameterMetadata(frameMetadata); if (!ok) { - wbtError << "Failed to store parameters metadata."; + bfError << "Failed to store parameters metadata."; return false; } @@ -111,21 +112,20 @@ bool Jacobian::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Matrix representing the Jacobian (6x(DoFs+6)) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple(InputIndex::BasePose, std::vector{4, 4}, DataType::DOUBLE), - std::make_tuple( - InputIndex::JointConfiguration, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::BasePose, Port::Dimensions{4, 4}, Port::DataType::DOUBLE}, + + {InputIndex::JointConfiguration, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple(OutputIndex::Jacobian, std::vector{6, 6 + dofs}, DataType::DOUBLE), - }, - }); + {OutputIndex::Jacobian, Port::Dimensions{6, 6 + dofs}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -142,13 +142,13 @@ bool Jacobian::initialize(BlockInformation* blockInfo) // ========== if (!Jacobian::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } std::string frame; if (!m_parameters.getParameter("Frame", frame)) { - wbtError << "Cannot retrieve string from frame parameter."; + bfError << "Cannot retrieve string from frame parameter."; return false; } @@ -160,14 +160,14 @@ bool Jacobian::initialize(BlockInformation* blockInfo) auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Cannot retrieve handle to KinDynComputations."; + bfError << "Cannot retrieve handle to KinDynComputations."; return false; } if (frame != "com") { pImpl->frameIndex = kinDyn->getFrameIndex(frame); if (pImpl->frameIndex == iDynTree::FRAME_INVALID_INDEX) { - wbtError << "Cannot find " + frame + " in the frame list."; + bfError << "Cannot find " + frame + " in the frame list."; return false; } } @@ -206,7 +206,7 @@ bool Jacobian::output(const BlockInformation* blockInfo) // Get the KinDynComputations object auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -217,14 +217,14 @@ bool Jacobian::output(const BlockInformation* blockInfo) InputSignalPtr jointsPosSig = blockInfo->getInputPortSignal(InputIndex::JointConfiguration); if (!basePoseSig || !jointsPosSig) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } bool ok = setRobotState(basePoseSig, jointsPosSig, nullptr, nullptr, kinDyn.get()); if (!ok) { - wbtError << "Failed to set the robot state."; + bfError << "Failed to set the robot state."; return false; } @@ -248,14 +248,14 @@ bool Jacobian::output(const BlockInformation* blockInfo) } if (!ok) { - wbtError << "Failed to get the Jacobian."; + bfError << "Failed to get the Jacobian."; return false; } // Get the output signal memory location OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::Jacobian); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } @@ -263,8 +263,8 @@ bool Jacobian::output(const BlockInformation* blockInfo) Map jacobianRowMajor = toEigen(pImpl->jacobian); Map jacobianColMajor( output->getBuffer(), - blockInfo->getOutputPortMatrixSize(OutputIndex::Jacobian).first, - blockInfo->getOutputPortMatrixSize(OutputIndex::Jacobian).second); + blockInfo->getOutputPortMatrixSize(OutputIndex::Jacobian).rows, + blockInfo->getOutputPortMatrixSize(OutputIndex::Jacobian).cols); // Forward the buffer to Simulink transforming it to ColMajor jacobianColMajor = jacobianRowMajor; diff --git a/toolbox/library/src/MassMatrix.cpp b/toolbox/library/src/MassMatrix.cpp index 2fda886f9..3e48f1ceb 100644 --- a/toolbox/library/src/MassMatrix.cpp +++ b/toolbox/library/src/MassMatrix.cpp @@ -6,13 +6,13 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "MassMatrix.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/MassMatrix.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" +#include +#include +#include #include #include #include @@ -21,7 +21,8 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -76,22 +77,19 @@ bool MassMatrix::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Matrix representing the mass matrix (DoFs+6)x(DoFs+6) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple(InputIndex::BasePose, std::vector{4, 4}, DataType::DOUBLE), - std::make_tuple( - InputIndex::JointConfiguration, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::BasePose, Port::Dimensions{4, 4}, Port::DataType::DOUBLE}, + {InputIndex::JointConfiguration, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple( - OutputIndex::MassMatrix, std::vector{dofs + 6, dofs + 6}, DataType::DOUBLE), - }, - }); + {OutputIndex::MassMatrix, Port::Dimensions{dofs + 6, dofs + 6}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -134,12 +132,12 @@ bool MassMatrix::output(const BlockInformation* blockInfo) // Get the KinDynComputations object auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -150,14 +148,14 @@ bool MassMatrix::output(const BlockInformation* blockInfo) InputSignalPtr jointsPosSig = blockInfo->getInputPortSignal(InputIndex::JointConfiguration); if (!basePoseSig || !jointsPosSig) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } bool ok = setRobotState(basePoseSig, jointsPosSig, nullptr, nullptr, kinDyn.get()); if (!ok) { - wbtError << "Failed to set the robot state."; + bfError << "Failed to set the robot state."; return false; } @@ -170,7 +168,7 @@ bool MassMatrix::output(const BlockInformation* blockInfo) // Get the output signal memory location OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::MassMatrix); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } @@ -178,8 +176,8 @@ bool MassMatrix::output(const BlockInformation* blockInfo) Map massMatrixRowMajor = toEigen(pImpl->massMatrix); Map massMatrixColMajor( output->getBuffer(), - blockInfo->getOutputPortMatrixSize(OutputIndex::MassMatrix).first, - blockInfo->getOutputPortMatrixSize(OutputIndex::MassMatrix).second); + blockInfo->getOutputPortMatrixSize(OutputIndex::MassMatrix).rows, + blockInfo->getOutputPortMatrixSize(OutputIndex::MassMatrix).cols); // Forward the buffer to Simulink transforming it to ColMajor massMatrixColMajor = massMatrixRowMajor; diff --git a/toolbox/library/src/MinimumJerkTrajectoryGenerator.cpp b/toolbox/library/src/MinimumJerkTrajectoryGenerator.cpp index 75cfc3cf2..684ad017c 100644 --- a/toolbox/library/src/MinimumJerkTrajectoryGenerator.cpp +++ b/toolbox/library/src/MinimumJerkTrajectoryGenerator.cpp @@ -6,13 +6,13 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "MinimumJerkTrajectoryGenerator.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/MinimumJerkTrajectoryGenerator.h" +#include +#include +#include +#include +#include #include #include @@ -20,7 +20,8 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -43,8 +44,8 @@ enum InputIndex // Other optional inputs }; -static int InputIndex_InitialValue = InputIndex::InputSignal; -static int InputIndex_ExtSettlingTime = InputIndex::InputSignal; +static size_t InputIndex_InitialValue = InputIndex::InputSignal; +static size_t InputIndex_ExtSettlingTime = InputIndex::InputSignal; enum OutputIndex { @@ -52,8 +53,8 @@ enum OutputIndex // Other optional outputs }; -static int OutputIndex_FirstDer = OutputIndex::FilteredSignal; -static int OutputIndex_SecondDer = OutputIndex::FilteredSignal; +static size_t OutputIndex_FirstDer = OutputIndex::FilteredSignal; +static size_t OutputIndex_SecondDer = OutputIndex::FilteredSignal; // BLOCK PIMPL // =========== @@ -102,7 +103,7 @@ bool MinimumJerkTrajectoryGenerator::parseParameters(BlockInformation* blockInfo for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -116,7 +117,7 @@ bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blo // ========== if (!MinimumJerkTrajectoryGenerator::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -132,7 +133,7 @@ bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blo ok = ok && m_parameters.getParameter("ReadExternalSettlingTime", readExternalSettlingTime); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -167,33 +168,37 @@ bool MinimumJerkTrajectoryGenerator::configureSizeAndPorts(BlockInformation* blo OutputIndex_SecondDer = OutputIndex_FirstDer + 1; } - BlockInformation::IOData ioData; - ioData.input.emplace_back( - InputIndex::InputSignal, std::vector{Signal::DynamicSize}, DataType::DOUBLE); - ioData.output.emplace_back( - OutputIndex::FilteredSignal, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + InputPortsInfo inputPortsInfo; + OutputPortsInfo outputPortsInfo; + + inputPortsInfo.push_back( + {InputIndex::InputSignal, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + outputPortsInfo.push_back( + {OutputIndex::FilteredSignal, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); // Handle optional inputs if (readInitialValue) { - ioData.input.emplace_back(InputIndex_InitialValue, std::vector{1}, DataType::DOUBLE); + inputPortsInfo.push_back( + {InputIndex_InitialValue, Port::Dimensions{1}, Port::DataType::DOUBLE}); } if (readExternalSettlingTime) { - ioData.input.emplace_back( - InputIndex_ExtSettlingTime, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back({InputIndex_ExtSettlingTime, + Port::Dimensions{Port::DynamicSize}, + Port::DataType::DOUBLE}); } // Handle optional outputs if (computeFirstDerivative) { - ioData.output.emplace_back( - OutputIndex_FirstDer, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + outputPortsInfo.push_back( + {OutputIndex_FirstDer, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); } if (computeSecondDerivative) { - ioData.output.emplace_back( - OutputIndex_SecondDer, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + outputPortsInfo.push_back( + {OutputIndex_SecondDer, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); } - if (!blockInfo->setIOPortsData(ioData)) { - wbtError << "Failed to configure input / output ports."; + if (!blockInfo->setPortsInfo(inputPortsInfo, outputPortsInfo)) { + bfError << "Failed to configure input / output ports."; return false; } @@ -210,7 +215,7 @@ bool MinimumJerkTrajectoryGenerator::initialize(BlockInformation* blockInfo) // ========== if (!MinimumJerkTrajectoryGenerator::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -230,7 +235,7 @@ bool MinimumJerkTrajectoryGenerator::initialize(BlockInformation* blockInfo) && m_parameters.getParameter("ResetOnSettlingTimeChange", pImpl->resetOnSettlingTimeChange); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -256,7 +261,7 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) InputSignalPtr externalTimeSignal = blockInfo->getInputPortSignal(InputIndex_ExtSettlingTime); if (!externalTimeSignal) { - wbtError << "Input signal not valid."; + bfError << "Input signal not valid."; return false; } @@ -277,7 +282,7 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) InputSignalPtr initialValuesSignal = blockInfo->getInputPortSignal(InputIndex_InitialValue); if (!initialValuesSignal) { - wbtError << "Input signal not valid."; + bfError << "Input signal not valid."; return false; } @@ -292,7 +297,7 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) InputSignalPtr referencesSignal = blockInfo->getInputPortSignal(InputIndex::InputSignal); if (!referencesSignal) { - wbtError << "Input signal not valid."; + bfError << "Input signal not valid."; return false; } @@ -309,12 +314,12 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) OutputSignalPtr outputSignal = blockInfo->getOutputPortSignal(OutputIndex::FilteredSignal); if (!outputSignal) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } if (!outputSignal->setBuffer(signal.data(), signal.size())) { - wbtError << "Failed to set output buffer."; + bfError << "Failed to set output buffer."; return false; } @@ -326,13 +331,13 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) OutputSignalPtr firstDerivativeSignal = blockInfo->getOutputPortSignal(OutputIndex_FirstDer); if (!firstDerivativeSignal) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } if (!firstDerivativeSignal->setBuffer(derivative.data(), firstDerivativeSignal->getWidth())) { - wbtError << "Failed to set output buffer."; + bfError << "Failed to set output buffer."; return false; } } @@ -345,13 +350,13 @@ bool MinimumJerkTrajectoryGenerator::output(const BlockInformation* blockInfo) OutputSignalPtr secondDerivativeSignal = blockInfo->getOutputPortSignal(OutputIndex_SecondDer); if (!secondDerivativeSignal) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } if (!secondDerivativeSignal->setBuffer(derivative.data(), secondDerivativeSignal->getWidth())) { - wbtError << "Failed to set output buffer."; + bfError << "Failed to set output buffer."; return false; } } diff --git a/toolbox/library/src/QpOases.cpp b/toolbox/library/src/QpOases.cpp index 902f4a1cd..9328b06c9 100644 --- a/toolbox/library/src/QpOases.cpp +++ b/toolbox/library/src/QpOases.cpp @@ -6,20 +6,21 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "QpOases.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/QpOases.h" +#include +#include +#include +#include +#include #include #include #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; const unsigned MaxIterations = 100; @@ -44,11 +45,11 @@ enum InputIndex // Other optional inputs }; -static int InputIndex_constraints = InputIndex::Gradient; -static int InputIndex_lbA = InputIndex::Gradient; -static int InputIndex_ubA = InputIndex::Gradient; -static int InputIndex_lb = InputIndex::Gradient; -static int InputIndex_ub = InputIndex::Gradient; +static size_t InputIndex_constraints = InputIndex::Gradient; +static size_t InputIndex_lbA = InputIndex::Gradient; +static size_t InputIndex_ubA = InputIndex::Gradient; +static size_t InputIndex_lb = InputIndex::Gradient; +static size_t InputIndex_ub = InputIndex::Gradient; enum OutputIndex { @@ -57,7 +58,7 @@ enum OutputIndex // Other optional inputs }; -static int OutputIndex_objVal = OutputIndex::Status; +static size_t OutputIndex_objVal = OutputIndex::Status; // BLOCK PIMPL // =========== @@ -105,7 +106,7 @@ bool QpOases::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -119,7 +120,7 @@ bool QpOases::configureSizeAndPorts(BlockInformation* blockInfo) // ========== if (!QpOases::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -137,7 +138,7 @@ bool QpOases::configureSizeAndPorts(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("ComputeObjVal", computeObjVal); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -159,56 +160,58 @@ bool QpOases::configureSizeAndPorts(BlockInformation* blockInfo) // 2) Status of the qp solver (1x1) // 3) Optional: Value of the object function (1x1) - BlockInformation::IOData ioData; + InputPortsInfo inputPortsInfo; + OutputPortsInfo outputPortsInfo; // Inputs - ioData.input.emplace_back(InputIndex::Hessian, - std::vector{Signal::DynamicSize, Signal::DynamicSize}, - DataType::DOUBLE); - ioData.input.emplace_back( - InputIndex::Gradient, std::vector{Signal::DynamicSize}, DataType::DOUBLE); - ioData.output.emplace_back( - OutputIndex::PrimalSolution, std::vector{Signal::DynamicSize}, DataType::DOUBLE); - ioData.output.emplace_back(OutputIndex::Status, std::vector{1}, DataType::DOUBLE); + inputPortsInfo.push_back({InputIndex::Hessian, + Port::Dimensions{Port::DynamicSize, Port::DynamicSize}, + Port::DataType::DOUBLE}); + inputPortsInfo.push_back( + {InputIndex::Gradient, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + outputPortsInfo.push_back( + {OutputIndex::PrimalSolution, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); + outputPortsInfo.push_back({OutputIndex::Status, Port::Dimensions{1}, Port::DataType::DOUBLE}); // Optional inputs - int numberOfInputs = InputIndex::Gradient; + size_t numberOfInputs = InputIndex::Gradient; if (useLbA || useUbA) { InputIndex_constraints = ++numberOfInputs; - ioData.input.emplace_back(InputIndex_constraints, - std::vector{Signal::DynamicSize, Signal::DynamicSize}, - DataType::DOUBLE); + inputPortsInfo.push_back({InputIndex_constraints, + Port::Dimensions{Port::DynamicSize, Port::DynamicSize}, + Port::DataType::DOUBLE}); } if (useLbA) { InputIndex_lbA = ++numberOfInputs; - ioData.input.emplace_back( - InputIndex_lbA, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back( + {InputIndex_lbA, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); } if (useUbA) { InputIndex_ubA = ++numberOfInputs; - ioData.input.emplace_back( - InputIndex_ubA, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back( + {InputIndex_ubA, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); } if (useLb) { InputIndex_lb = ++numberOfInputs; - ioData.input.emplace_back( - InputIndex_lb, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back( + {InputIndex_lb, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); } if (useUb) { InputIndex_ub = ++numberOfInputs; - ioData.input.emplace_back( - InputIndex_ub, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back( + {InputIndex_ub, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}); } // Optional outputs - int numberOfOutputs = OutputIndex::Status; + size_t numberOfOutputs = OutputIndex::Status; if (computeObjVal) { OutputIndex_objVal = ++numberOfOutputs; - ioData.output.emplace_back(OutputIndex_objVal, std::vector{1}, DataType::DOUBLE); + outputPortsInfo.push_back( + {OutputIndex_objVal, Port::Dimensions{1}, Port::DataType::DOUBLE}); } - if (!blockInfo->setIOPortsData(ioData)) { - wbtError << "Failed to configure input / output ports."; + if (!blockInfo->setPortsInfo(inputPortsInfo, outputPortsInfo)) { + bfError << "Failed to configure input / output ports."; return false; } @@ -225,7 +228,7 @@ bool QpOases::initialize(BlockInformation* blockInfo) // ========== if (!QpOases::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -238,7 +241,7 @@ bool QpOases::initialize(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("StopWhenFails", pImpl->stopWhenFails); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -247,16 +250,16 @@ bool QpOases::initialize(BlockInformation* blockInfo) // Check the hessian size const auto size_H = blockInfo->getInputPortMatrixSize(InputIndex::Hessian); - const auto numberOfVariables = size_H.first; - if (size_H.first != size_H.second) { - wbtError << "The Hessian matrix should be square."; + const auto numberOfVariables = size_H.rows; + if (size_H.rows != size_H.cols) { + bfError << "The Hessian matrix should be square."; return false; } // Check the gradient size const auto size_g = blockInfo->getInputPortWidth(InputIndex::Gradient); if (size_g != numberOfVariables) { - wbtError << "The gradient size does not match with the Hessian size."; + bfError << "The gradient size does not match with the Hessian size."; return false; } @@ -269,7 +272,7 @@ bool QpOases::initialize(BlockInformation* blockInfo) ok = ok && (blockInfo->getInputPortWidth(InputIndex_ub) == numberOfVariables); } if (!ok) { - wbtError << "Sizes of bounds do not match with the number of variables."; + bfError << "Sizes of bounds do not match with the number of variables."; return false; } @@ -278,15 +281,15 @@ bool QpOases::initialize(BlockInformation* blockInfo) if (pImpl->useLbA || pImpl->useUbA) { // Check the constraints size const auto size_c = blockInfo->getInputPortMatrixSize(InputIndex_constraints); - numberOfConstraints = size_c.first; - if (size_c.second != numberOfVariables) { - wbtError << "The column size of the constraints matrix does not match with " - << "the Hessian size"; + numberOfConstraints = size_c.rows; + if (size_c.cols != numberOfVariables) { + bfError << "The column size of the constraints matrix does not match with " + << "the Hessian size"; return false; } // Resize the buffer - pImpl->constraints_rowMajor.resize(size_c.first, size_c.second); + pImpl->constraints_rowMajor.resize(size_c.rows, size_c.cols); // Check the constraints' bound size bool ok = true; @@ -297,7 +300,7 @@ bool QpOases::initialize(BlockInformation* blockInfo) ok = ok && (blockInfo->getInputPortWidth(InputIndex_ubA) == numberOfConstraints); } if (!ok) { - wbtError << "Sizes of constraints' bounds do not match with the number of constraints."; + bfError << "Sizes of constraints' bounds do not match with the number of constraints."; return false; } } @@ -309,7 +312,7 @@ bool QpOases::initialize(BlockInformation* blockInfo) new qpOASES::SQProblem(numberOfVariables, numberOfConstraints)); if (!pImpl->sqProblem) { - wbtError << "Failed to allocate the QProblem or SQProblem object."; + bfError << "Failed to allocate the QProblem or SQProblem object."; return false; } @@ -344,7 +347,7 @@ bool QpOases::output(const BlockInformation* blockInfo) InputSignalPtr gradientSignal = blockInfo->getInputPortSignal(InputIndex::Gradient); if (!hessianSignal || !gradientSignal) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } @@ -360,7 +363,7 @@ bool QpOases::output(const BlockInformation* blockInfo) if (pImpl->useLbA || pImpl->useUbA) { InputSignalPtr constraintsSignal = blockInfo->getInputPortSignal(InputIndex_constraints); if (!constraintsSignal) { - wbtError << "Signal for lbA is not valid."; + bfError << "Signal for lbA is not valid."; return false; } @@ -370,8 +373,8 @@ bool QpOases::output(const BlockInformation* blockInfo) Map constraints_colMajor( const_cast(constraintsSignal->getBuffer()), - blockInfo->getInputPortMatrixSize(InputIndex_constraints).first, - blockInfo->getInputPortMatrixSize(InputIndex_constraints).second); + blockInfo->getInputPortMatrixSize(InputIndex_constraints).rows, + blockInfo->getInputPortMatrixSize(InputIndex_constraints).cols); pImpl->constraints_rowMajor = constraints_colMajor; // Update the buffer passed to SQProblem @@ -381,7 +384,7 @@ bool QpOases::output(const BlockInformation* blockInfo) InputSignalPtr lbASignal = blockInfo->getInputPortSignal(InputIndex_lbA); lbA = lbASignal->getBuffer(); if (!lbASignal) { - wbtError << "Signal for lbA is not valid."; + bfError << "Signal for lbA is not valid."; return false; } } @@ -390,7 +393,7 @@ bool QpOases::output(const BlockInformation* blockInfo) InputSignalPtr ubASignal = blockInfo->getInputPortSignal(InputIndex_ubA); ubA = ubASignal->getBuffer(); if (!ubASignal) { - wbtError << "Signal for ubA is not valid."; + bfError << "Signal for ubA is not valid."; return false; } } @@ -400,7 +403,7 @@ bool QpOases::output(const BlockInformation* blockInfo) InputSignalPtr lbSignal = blockInfo->getInputPortSignal(InputIndex_lb); lb = lbSignal->getBuffer(); if (!lbSignal) { - wbtError << "Signal for lb is not valid."; + bfError << "Signal for lb is not valid."; return false; } } @@ -409,7 +412,7 @@ bool QpOases::output(const BlockInformation* blockInfo) InputSignalPtr ubSignal = blockInfo->getInputPortSignal(InputIndex_ub); ub = ubSignal->getBuffer(); if (!ubSignal) { - wbtError << "Signal for ub is not valid."; + bfError << "Signal for ub is not valid."; return false; } } @@ -419,13 +422,13 @@ bool QpOases::output(const BlockInformation* blockInfo) OutputSignalPtr solutionSignal = blockInfo->getOutputPortSignal(OutputIndex::PrimalSolution); if (!solutionSignal) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } OutputSignalPtr statusSignal = blockInfo->getOutputPortSignal(OutputIndex::Status); if (!statusSignal) { - wbtError << "Status signal not valid."; + bfError << "Status signal not valid."; return false; } @@ -444,7 +447,7 @@ bool QpOases::output(const BlockInformation* blockInfo) nWSR, nullptr); if (pImpl->stopWhenFails && status != qpOASES::SUCCESSFUL_RETURN) { - wbtError << "qpOASES: init() failed."; + bfError << "qpOASES: init() failed."; return false; } } @@ -462,8 +465,8 @@ bool QpOases::output(const BlockInformation* blockInfo) // Handle possible errors if ((status != qpOASES::SUCCESSFUL_RETURN) && (status != qpOASES::RET_MAX_NWSR_REACHED)) { - wbtWarning << "Internal qpOASES error. Trying to solve the problem with the remaining " - << "number of iterations."; + bfWarning << "Internal qpOASES error. Trying to solve the problem with the remaining " + << "number of iterations."; pImpl->sqProblem->reset(); nWSR = MaxIterations - nWSR; @@ -479,7 +482,7 @@ bool QpOases::output(const BlockInformation* blockInfo) } if (pImpl->stopWhenFails && status != qpOASES::SUCCESSFUL_RETURN) { - wbtError << "qpOASES: hotstart() failed."; + bfError << "qpOASES: hotstart() failed."; return false; } } @@ -488,12 +491,12 @@ bool QpOases::output(const BlockInformation* blockInfo) pImpl->sqProblem->getPrimalSolution(solutionSignal->getBuffer()); if (pImpl->stopWhenFails && statusSol != qpOASES::SUCCESSFUL_RETURN) { - wbtError << "qpOASES: getPrimalSolution() failed."; + bfError << "qpOASES: getPrimalSolution() failed."; return false; } if (!statusSignal->set(0, qpOASES::getSimpleStatus(status))) { - wbtError << "Failed to set status signal."; + bfError << "Failed to set status signal."; return false; } @@ -505,12 +508,12 @@ bool QpOases::output(const BlockInformation* blockInfo) OutputSignalPtr objValSignal = blockInfo->getOutputPortSignal(OutputIndex_objVal); if (!objValSignal) { - wbtError << "Object Value signal not valid."; + bfError << "Object Value signal not valid."; return false; } if (!objValSignal->set(0, objVal)) { - wbtError << "Failed to set object value signal."; + bfError << "Failed to set object value signal."; return false; } } diff --git a/toolbox/library/src/RealTimeSynchronizer.cpp b/toolbox/library/src/RealTimeSynchronizer.cpp index 30c4a1cc2..fad51c85a 100644 --- a/toolbox/library/src/RealTimeSynchronizer.cpp +++ b/toolbox/library/src/RealTimeSynchronizer.cpp @@ -6,19 +6,20 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "RealTimeSynchronizer.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" +#include "WBToolbox/Block/RealTimeSynchronizer.h" +#include +#include +#include +#include #include #include #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -60,7 +61,7 @@ bool RealTimeSynchronizer::parseParameters(BlockInformation* blockInfo) ParameterType::DOUBLE, ParamIndex::Period, 1, 1, "Period"); if (!blockInfo->addParameterMetadata(periodMetadata)) { - wbtError << "Failed to store parameters metadata."; + bfError << "Failed to store parameters metadata."; return false; } @@ -80,10 +81,10 @@ bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) // No outputs // - const bool ok = blockInfo->setIOPortsData({{}, {}}); + const bool ok = blockInfo->setPortsInfo({}, {}); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -100,17 +101,17 @@ bool RealTimeSynchronizer::initialize(BlockInformation* blockInfo) // ========== if (!RealTimeSynchronizer::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } if (!m_parameters.getParameter("Period", pImpl->period)) { - wbtError << "Failed to get parameter 'period' after its parsing."; + bfError << "Failed to get parameter 'period' after its parsing."; return false; } if (pImpl->period <= 0) { - wbtError << "Period must be greater than 0."; + bfError << "Period must be greater than 0."; return false; } @@ -119,7 +120,7 @@ bool RealTimeSynchronizer::initialize(BlockInformation* blockInfo) yarp::os::Network::init(); if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { - wbtError << "YARP server wasn't found active!!"; + bfError << "YARP server wasn't found active!!"; return false; } diff --git a/toolbox/library/src/RelativeTransform.cpp b/toolbox/library/src/RelativeTransform.cpp index d6a855f03..0b44c3152 100644 --- a/toolbox/library/src/RelativeTransform.cpp +++ b/toolbox/library/src/RelativeTransform.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "RelativeTransform.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/RelativeTransform.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -23,14 +23,15 @@ #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, Frame1, Frame2 }; @@ -78,7 +79,7 @@ bool RelativeTransform::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -106,20 +107,19 @@ bool RelativeTransform::configureSizeAndPorts(BlockInformation* blockInfo) // 1) Homogeneous transformation between frame1 and frame2 (4x4 matrix) // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple( - InputIndex::JointConfiguration, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::JointConfiguration, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, }, { // Outputs - std::make_tuple(OutputIndex::Transform, std::vector{4, 4}, DataType::DOUBLE), - }, - }); + {OutputIndex::Transform, Port::Dimensions{4, 4}, Port::DataType::DOUBLE}, + + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -136,7 +136,7 @@ bool RelativeTransform::initialize(BlockInformation* blockInfo) // ========== if (!parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -148,7 +148,7 @@ bool RelativeTransform::initialize(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("Frame2", frame2); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -160,21 +160,21 @@ bool RelativeTransform::initialize(BlockInformation* blockInfo) const auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Cannot retrieve handle to KinDynComputations."; + bfError << "Cannot retrieve handle to KinDynComputations."; return false; } // Frame 1 pImpl->frame1Index = kinDyn->getFrameIndex(frame1); if (pImpl->frame1Index == iDynTree::FRAME_INVALID_INDEX) { - wbtError << "Cannot find " + frame1 + " in the frame list."; + bfError << "Cannot find " + frame1 + " in the frame list."; return false; } // Frame 2 pImpl->frame2Index = kinDyn->getFrameIndex(frame2); if (pImpl->frame2Index == iDynTree::FRAME_INVALID_INDEX) { - wbtError << "Cannot find " + frame2 + " in the frame list."; + bfError << "Cannot find " + frame2 + " in the frame list."; return false; } @@ -199,7 +199,7 @@ bool RelativeTransform::output(const BlockInformation* blockInfo) // Get the KinDynComputations object auto kinDyn = getKinDynComputations(); if (!kinDyn) { - wbtError << "Failed to retrieve the KinDynComputations object."; + bfError << "Failed to retrieve the KinDynComputations object."; return false; } @@ -209,7 +209,7 @@ bool RelativeTransform::output(const BlockInformation* blockInfo) InputSignalPtr jointsPosSig = blockInfo->getInputPortSignal(InputIndex::JointConfiguration); if (!jointsPosSig) { - wbtError << "Input signals not valid."; + bfError << "Input signals not valid."; return false; } @@ -230,7 +230,7 @@ bool RelativeTransform::output(const BlockInformation* blockInfo) // Get the output signal memory location OutputSignalPtr output = blockInfo->getOutputPortSignal(OutputIndex::Transform); if (!output) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } diff --git a/toolbox/library/src/SetMotorParameters.cpp b/toolbox/library/src/SetMotorParameters.cpp index d2575cb28..79b59e92e 100644 --- a/toolbox/library/src/SetMotorParameters.cpp +++ b/toolbox/library/src/SetMotorParameters.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "Block/SetMotorParameters.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/SetMotorParameters.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -26,14 +26,15 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, SetP, SetI, SetD, @@ -99,7 +100,7 @@ bool SetMotorParameters::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -117,7 +118,7 @@ bool SetMotorParameters::configureSizeAndPorts(BlockInformation* blockInfo) // ========== if (!SetMotorParameters::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -131,7 +132,7 @@ bool SetMotorParameters::configureSizeAndPorts(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("SetD", setD); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -149,33 +150,36 @@ bool SetMotorParameters::configureSizeAndPorts(BlockInformation* blockInfo) // int numberOfInputs = -1; - BlockInformation::IOData ioData; + InputPortsInfo inputPortsInfo; if (setP) { InputIndex_PGains = ++numberOfInputs; - ioData.input.emplace_back( - InputIndex_PGains, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back({static_cast(InputIndex_PGains), + Port::Dimensions{Port::DynamicSize}, + Port::DataType::DOUBLE}); } if (setI) { InputIndex_IGains = ++numberOfInputs; - ioData.input.emplace_back( - InputIndex_IGains, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back({static_cast(InputIndex_IGains), + Port::Dimensions{Port::DynamicSize}, + Port::DataType::DOUBLE}); } if (setD) { InputIndex_DGains = ++numberOfInputs; - ioData.input.emplace_back( - InputIndex_DGains, std::vector{Signal::DynamicSize}, DataType::DOUBLE); + inputPortsInfo.push_back({static_cast(InputIndex_DGains), + Port::Dimensions{Port::DynamicSize}, + Port::DataType::DOUBLE}); } - if (!blockInfo->setIOPortsData(ioData)) { - wbtError << "Failed to configure input / output ports."; + if (!blockInfo->setPortsInfo(inputPortsInfo, {})) { + bfError << "Failed to configure input / output ports."; return false; } if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -192,7 +196,7 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) // ================ if (!SetMotorParameters::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -211,7 +215,7 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("Bemf", bemfVector); if (!ok) { - wbtError << "Failed to get parameters after their parsing."; + bfError << "Failed to get parameters after their parsing."; return false; } @@ -223,12 +227,12 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) const auto dofs = getRobotInterface()->getConfiguration().getNumberOfDoFs(); if (pImpl->setKTau && (kTauVector.size() != dofs)) { - wbtError << "KTau vector doesn't have a width equal to " << dofs << "."; + bfError << "KTau vector doesn't have a width equal to " << dofs << "."; return false; } if (pImpl->setBemf && (bemfVector.size() != dofs)) { - wbtError << "Back EMF vector doesn't have a width equal to " << dofs << "."; + bfError << "Back EMF vector doesn't have a width equal to " << dofs << "."; return false; } @@ -242,7 +246,7 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) pImpl->controlType = yarp::dev::VOCAB_PIDTYPE_TORQUE; } else { - wbtError << "Control type not recognized."; + bfError << "Control type not recognized."; return false; } @@ -252,13 +256,13 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) // Get the interface yarp::dev::IPidControl* iPidControl = nullptr; if (!robotInterface->getInterface(iPidControl) || !iPidControl) { - wbtError << "Failed to get IPidControl interface."; + bfError << "Failed to get IPidControl interface."; return false; } // Store the default gains if (!iPidControl->getPids(pImpl->controlType, pImpl->pidValuesDefault.data())) { - wbtError << "Failed to get default data from IPidControl."; + bfError << "Failed to get default data from IPidControl."; return false; } @@ -271,7 +275,7 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) // Get the interface yarp::dev::ITorqueControl* iTorqueControl = nullptr; if (!getRobotInterface()->getInterface(iTorqueControl) || !iTorqueControl) { - wbtError << "Failed to get ITorqueControl interface."; + bfError << "Failed to get ITorqueControl interface."; return false; } @@ -282,7 +286,7 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) // Get the default values for (unsigned m = 0; m < dofs; ++m) { if (!iTorqueControl->getMotorTorqueParams(m, &pImpl->motorParamsDefault[m])) { - wbtError << "Failed to get motor torque parameters."; + bfError << "Failed to get motor torque parameters."; return false; } } @@ -306,7 +310,7 @@ bool SetMotorParameters::initialize(BlockInformation* blockInfo) if ((pImpl->setP && (blockInfo->getInputPortWidth(InputIndex_PGains) != dofs)) || (pImpl->setI && (blockInfo->getInputPortWidth(InputIndex_IGains) != dofs)) || (pImpl->setD && (blockInfo->getInputPortWidth(InputIndex_DGains) != dofs))) { - wbtError << "Input ports must have a size equal to " << dofs << "."; + bfError << "Input ports must have a size equal to " << dofs << "."; return false; } @@ -322,27 +326,27 @@ bool SetMotorParameters::terminate(const BlockInformation* blockInfo) // Get the IPidControl interface yarp::dev::IPidControl* iPidControl = nullptr; if (!robotInterface->getInterface(iPidControl) || !iPidControl) { - wbtError << "Failed to get IPidControl interface."; + bfError << "Failed to get IPidControl interface."; return false; } // Reset default PID gains if (!iPidControl->setPids(pImpl->controlType, pImpl->pidValuesDefault.data())) { - wbtError << "Failed to reset PIDs to the default values."; + bfError << "Failed to reset PIDs to the default values."; return false; } // Get the ITorqueControl interface yarp::dev::ITorqueControl* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get ITorqueControl interface."; + bfError << "Failed to get ITorqueControl interface."; return false; } // Restore default motor torque parameters for (unsigned m = 0; m < dofs; ++m) { if (!interface->setMotorTorqueParams(m, pImpl->motorParamsDefault[m])) { - wbtError << "Failed to restore default motor torque parameters."; + bfError << "Failed to restore default motor torque parameters."; break; } } @@ -365,15 +369,15 @@ bool SetMotorParameters::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::ITorqueControl* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get ITorqueControl interface."; + bfError << "Failed to get ITorqueControl interface."; return false; } // Apply the motor parameters for (unsigned m = 0; m < dofs; ++m) { if (!interface->setMotorTorqueParams(m, pImpl->motorParamsApplied[m])) { - wbtError << "Failed to set motor torque parameters for joint " - << controlledJoints[m] << "."; + bfError << "Failed to set motor torque parameters for joint " << controlledJoints[m] + << "."; break; } } @@ -385,7 +389,7 @@ bool SetMotorParameters::output(const BlockInformation* blockInfo) InputSignalPtr pGainsSignal = blockInfo->getInputPortSignal(InputIndex_PGains); if (!pGainsSignal) { - wbtError << "Failed to get signal containing proportional gains."; + bfError << "Failed to get signal containing proportional gains."; } for (unsigned i = 0; i < pImpl->pidValuesApplied.size(); ++i) { @@ -401,7 +405,7 @@ bool SetMotorParameters::output(const BlockInformation* blockInfo) InputSignalPtr iGainsSignal = blockInfo->getInputPortSignal(InputIndex_IGains); if (!iGainsSignal) { - wbtError << "Failed to get signal containing integral gains."; + bfError << "Failed to get signal containing integral gains."; } for (unsigned i = 0; i < pImpl->pidValuesApplied.size(); ++i) { @@ -417,7 +421,7 @@ bool SetMotorParameters::output(const BlockInformation* blockInfo) InputSignalPtr dGainsSignal = blockInfo->getInputPortSignal(InputIndex_DGains); if (!dGainsSignal) { - wbtError << "Failed to get signal containing derivative gains."; + bfError << "Failed to get signal containing derivative gains."; } for (unsigned i = 0; i < pImpl->pidValuesApplied.size(); ++i) { @@ -433,13 +437,13 @@ bool SetMotorParameters::output(const BlockInformation* blockInfo) // Get the interface yarp::dev::IPidControl* iPidControl = nullptr; if (!robotInterface->getInterface(iPidControl) || !iPidControl) { - wbtError << "Failed to get IPidControl interface."; + bfError << "Failed to get IPidControl interface."; return false; } // Apply the new pid gains if (!iPidControl->setPids(pImpl->controlType, pImpl->pidValuesApplied.data())) { - wbtError << "Failed to set PID values."; + bfError << "Failed to set PID values."; return false; } } diff --git a/toolbox/library/src/SetReferences.cpp b/toolbox/library/src/SetReferences.cpp index e2ef656ad..1d296bff7 100644 --- a/toolbox/library/src/SetReferences.cpp +++ b/toolbox/library/src/SetReferences.cpp @@ -6,15 +6,15 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "SetReferences.h" -#include "Base/Configuration.h" -#include "Base/RobotInterface.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" - +#include "WBToolbox/Block/SetReferences.h" +#include "WBToolbox/Base/Configuration.h" +#include "WBToolbox/Base/RobotInterface.h" + +#include +#include +#include +#include +#include #include #include #include @@ -29,14 +29,15 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== enum ParamIndex { - Bias = WBBlock::NumberOfParameters - 1, + Bias = wbt::base::WBBlock::NumberOfParameters - 1, CtrlType, TrajRef }; @@ -94,7 +95,7 @@ bool SetReferences::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -122,18 +123,17 @@ bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) // No outputs // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple(InputIndex::References, std::vector{dofs}, DataType::DOUBLE), + {InputIndex::References, Port::Dimensions{dofs}, Port::DataType::DOUBLE}, }, { // Outputs - }, - }); + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -153,18 +153,18 @@ bool SetReferences::initialize(BlockInformation* blockInfo) // ========== if (!SetReferences::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } std::string controlType; if (!m_parameters.getParameter("CtrlType", controlType)) { - wbtError << "Could not read control type parameter."; + bfError << "Could not read control type parameter."; return false; } if (!m_parameters.getParameter("TrajectoryReference", pImpl->trajectoryReference)) { - wbtError << "Could not read reference speed / acceleration parameter."; + bfError << "Could not read reference speed / acceleration parameter."; return false; } @@ -197,7 +197,7 @@ bool SetReferences::initialize(BlockInformation* blockInfo) pImpl->controlModes.assign(dofs, VOCAB_CM_CURRENT); } else { - wbtError << "Control Mode not supported."; + bfError << "Control Mode not supported."; return false; } @@ -212,21 +212,21 @@ bool SetReferences::initialize(BlockInformation* blockInfo) // Get the interface yarp::dev::IPositionControl* interface = nullptr; if (!getRobotInterface()->getInterface(interface) || !interface) { - wbtError << "Failed to get IPositionControl interface."; + bfError << "Failed to get IPositionControl interface."; return false; } // Store the default reference speeds pImpl->defaultTrajectoryReference.resize(dofs); if (!interface->getRefSpeeds(pImpl->defaultTrajectoryReference.data())) { - wbtError << "Failed to get default reference speed."; + bfError << "Failed to get default reference speed."; return false; } // Set the new reference speeds std::vector speedInitalization(dofs, pImpl->trajectoryReference); if (!interface->setRefSpeeds(speedInitalization.data())) { - wbtError << "Failed to initialize reference speed."; + bfError << "Failed to initialize reference speed."; return false; } @@ -246,21 +246,21 @@ bool SetReferences::initialize(BlockInformation* blockInfo) // Get the interface yarp::dev::IVelocityControl* interface = nullptr; if (!getRobotInterface()->getInterface(interface) || !interface) { - wbtError << "Failed to get IVelocityControl interface."; + bfError << "Failed to get IVelocityControl interface."; return false; } // Store the default reference accelerations pImpl->defaultTrajectoryReference.resize(dofs); if (!interface->getRefAccelerations(pImpl->defaultTrajectoryReference.data())) { - wbtError << "Failed to get default reference acceleration."; + bfError << "Failed to get default reference acceleration."; return false; } // Set the new reference accelerations std::vector speedInitalization(dofs, pImpl->trajectoryReference); if (!interface->setRefAccelerations(speedInitalization.data())) { - wbtError << "Failed to initialize reference acceleration."; + bfError << "Failed to initialize reference acceleration."; return false; } @@ -286,7 +286,7 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) IControlMode* icmd = nullptr; ok = robotInterface->getInterface(icmd); if (!ok || !icmd) { - wbtError << "Failed to get the IControlMode interface."; + bfError << "Failed to get the IControlMode interface."; return false; } @@ -296,14 +296,14 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) yarp::dev::IPositionControl* interface = nullptr; ok = robotInterface->getInterface(interface); if (!ok || !interface) { - wbtError << "Failed to get IPositionControl interface."; + bfError << "Failed to get IPositionControl interface."; return false; } // Restore default reference speeds if (interface) { ok = interface->setRefSpeeds(pImpl->defaultTrajectoryReference.data()); if (!ok) { - wbtError << "Failed to restore default reference speed."; + bfError << "Failed to restore default reference speed."; return false; } } @@ -315,14 +315,14 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) yarp::dev::IVelocityControl* interface = nullptr; ok = robotInterface->getInterface(interface); if (!ok || !interface) { - wbtError << "Failed to get IVelocityControl interface."; + bfError << "Failed to get IVelocityControl interface."; return false; } // Restore default reference accelerations if (interface) { ok = interface->setRefAccelerations(pImpl->defaultTrajectoryReference.data()); if (!ok) { - wbtError << "Failed to restore default reference acceleration."; + bfError << "Failed to restore default reference acceleration."; return false; } } @@ -334,7 +334,7 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) ok = icmd->setControlModes(pImpl->controlModes.data()); if (!ok) { - wbtError << "Failed to set control mode."; + bfError << "Failed to set control mode."; return false; } } @@ -372,12 +372,12 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the interface IControlMode* icmd = nullptr; if (!robotInterface->getInterface(icmd) || !icmd) { - wbtError << "Failed to get the IControlMode2 interface."; + bfError << "Failed to get the IControlMode2 interface."; return false; } // Set the control mode to all the controlledJoints if (!icmd->setControlModes(pImpl->controlModes.data())) { - wbtError << "Failed to set control mode."; + bfError << "Failed to set control mode."; return false; } } @@ -385,7 +385,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the signal InputSignalPtr references = blockInfo->getInputPortSignal(InputIndex::References); if (!references) { - wbtError << "Input signal not valid."; + bfError << "Input signal not valid."; return false; } @@ -393,7 +393,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) const double* bufferReferences = references->getBuffer(); if (!bufferReferences) { - wbtError << "Failed to get the buffer containing references."; + bfError << "Failed to get the buffer containing references."; return false; } @@ -401,7 +401,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // TODO: here only the first element is checked switch (pImpl->controlModes.front()) { case VOCAB_CM_UNKNOWN: - wbtError << "Control mode has not been successfully set."; + bfError << "Control mode has not been successfully set."; return false; case VOCAB_CM_POSITION: { // Do not update the position reference if it didn't change. @@ -417,7 +417,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the interface IPositionControl* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get IPositionControl interface."; + bfError << "Failed to get IPositionControl interface."; return false; } // Convert from rad to deg @@ -430,7 +430,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the interface IPositionDirect* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get IPositionDirect interface."; + bfError << "Failed to get IPositionDirect interface."; return false; } // Convert from rad to deg @@ -443,7 +443,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the interface IVelocityControl* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get IVelocityControl interface."; + bfError << "Failed to get IVelocityControl interface."; return false; } // Convert from rad to deg @@ -456,7 +456,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the interface ITorqueControl* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get ITorqueControl interface."; + bfError << "Failed to get ITorqueControl interface."; return false; } // Set the references @@ -467,7 +467,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the interface IPWMControl* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get IPWMControl interface."; + bfError << "Failed to get IPWMControl interface."; return false; } // Set the references @@ -478,7 +478,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) // Get the interface ICurrentControl* interface = nullptr; if (!robotInterface->getInterface(interface) || !interface) { - wbtError << "Failed to get ICurrentControl interface."; + bfError << "Failed to get ICurrentControl interface."; return false; } // Set the references @@ -488,7 +488,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) } if (!ok) { - wbtError << "Failed to set references."; + bfError << "Failed to set references."; return false; } diff --git a/toolbox/library/src/SimulatorSynchronizer.cpp b/toolbox/library/src/SimulatorSynchronizer.cpp index cf62e621a..f05093b19 100644 --- a/toolbox/library/src/SimulatorSynchronizer.cpp +++ b/toolbox/library/src/SimulatorSynchronizer.cpp @@ -6,20 +6,21 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "SimulatorSynchronizer.h" +#include "WBToolbox/Block/SimulatorSynchronizer.h" #include "ClockServer.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" +#include +#include +#include +#include #include #include #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -72,7 +73,7 @@ unsigned SimulatorSynchronizer::numberOfParameters() std::vector SimulatorSynchronizer::additionalBlockOptions() { - return std::vector(1, wbt::BlockOptionPrioritizeOrder); + return std::vector(1, blockfactory::core::BlockOptionPrioritizeOrder); } bool SimulatorSynchronizer::parseParameters(BlockInformation* blockInfo) @@ -84,7 +85,7 @@ bool SimulatorSynchronizer::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -105,10 +106,10 @@ bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) // No outputs // - const bool ok = blockInfo->setIOPortsData({{}, {}}); + const bool ok = blockInfo->setPortsInfo({}, {}); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -125,7 +126,7 @@ bool SimulatorSynchronizer::initialize(BlockInformation* blockInfo) // ========== if (!SimulatorSynchronizer::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -138,7 +139,7 @@ bool SimulatorSynchronizer::initialize(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("RpcPort", clientPortName); if (!ok) { - wbtError << "Error reading RPC parameters."; + bfError << "Error reading RPC parameters."; return false; } @@ -147,7 +148,7 @@ bool SimulatorSynchronizer::initialize(BlockInformation* blockInfo) yarp::os::Network::init(); if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork()) { - wbtError << "Error initializing Yarp network."; + bfError << "Error initializing Yarp network."; return false; } @@ -165,7 +166,7 @@ bool SimulatorSynchronizer::terminate(const BlockInformation* /*blockInfo*/) pImpl->rpcData.clockServer.continueSimulation(); if (!yarp::os::Network::disconnect(pImpl->rpcData.configuration.clientPortName, pImpl->rpcData.configuration.serverPortName)) { - wbtError << "Error disconnecting from simulator clock server."; + bfError << "Error disconnecting from simulator clock server."; } pImpl->rpcData.clientPort.close(); } @@ -182,7 +183,7 @@ bool SimulatorSynchronizer::output(const BlockInformation* /*blockInfo*/) if (!pImpl->rpcData.clientPort.open(pImpl->rpcData.configuration.clientPortName) || !yarp::os::Network::connect(pImpl->rpcData.configuration.clientPortName, pImpl->rpcData.configuration.serverPortName)) { - wbtError << "Error connecting to simulator clock server."; + bfError << "Error connecting to simulator clock server."; return false; } diff --git a/toolbox/library/src/YarpClock.cpp b/toolbox/library/src/YarpClock.cpp index 5a2e0b30c..5a19c24dc 100644 --- a/toolbox/library/src/YarpClock.cpp +++ b/toolbox/library/src/YarpClock.cpp @@ -6,18 +6,19 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "YarpClock.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/YarpClock.h" +#include +#include +#include #include #include #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -49,18 +50,17 @@ bool YarpClock::configureSizeAndPorts(BlockInformation* blockInfo) // 1) The yarp time. In short, it streams yarp::os::Time::now(). // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs }, { // Outputs - std::make_tuple(OutputIndex::Clock, std::vector{1}, DataType::DOUBLE), - }, - }); + {OutputIndex::Clock, Port::Dimensions{1}, Port::DataType::DOUBLE}, + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -79,7 +79,7 @@ bool YarpClock::initialize(BlockInformation* blockInfo) yarp::os::Network::init(); if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { - wbtError << "YARP server wasn't found active."; + bfError << "YARP server wasn't found active."; return false; } @@ -96,12 +96,12 @@ bool YarpClock::output(const BlockInformation* blockInfo) { OutputSignalPtr outputSignal = blockInfo->getOutputPortSignal(OutputIndex::Clock); if (!outputSignal) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } if (!outputSignal->set(0, yarp::os::Time::now())) { - wbtError << "Failed to write data to the output signal."; + bfError << "Failed to write data to the output signal."; return false; } return true; diff --git a/toolbox/library/src/YarpRead.cpp b/toolbox/library/src/YarpRead.cpp index 0f48a433f..6d560a135 100644 --- a/toolbox/library/src/YarpRead.cpp +++ b/toolbox/library/src/YarpRead.cpp @@ -6,13 +6,13 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "YarpRead.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/YarpRead.h" +#include +#include +#include +#include +#include #include #include #include @@ -26,7 +26,8 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -49,8 +50,8 @@ enum OutputIndex // Other optional outputs }; -static int OutputIndex_Timestamp = OutputIndex::Signal; -static int OutputIndex_IsConnected = OutputIndex::Signal; +static size_t OutputIndex_Timestamp = OutputIndex::Signal; +static size_t OutputIndex_IsConnected = OutputIndex::Signal; // BLOCK PIMPL // =========== @@ -97,7 +98,7 @@ bool YarpRead::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -111,7 +112,7 @@ bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) // ========== if (!YarpRead::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -125,12 +126,12 @@ bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("SignalSize", signalSize); if (!ok) { - wbtError << "Failed to read input parameters."; + bfError << "Failed to read input parameters."; return false; } if (signalSize <= 0) { - wbtError << "Signal size must be non negative."; + bfError << "Signal size must be non negative."; return false; } @@ -156,19 +157,22 @@ bool YarpRead::configureSizeAndPorts(BlockInformation* blockInfo) OutputIndex_IsConnected = OutputIndex_Timestamp + 1; } - BlockInformation::IOData ioData; - ioData.output.emplace_back(OutputIndex::Signal, std::vector{signalSize}, DataType::DOUBLE); + OutputPortsInfo outputPortsInfo; + outputPortsInfo.push_back( + {OutputIndex::Signal, Port::Dimensions{signalSize}, Port::DataType::DOUBLE}); if (readTimestamp) { - ioData.output.emplace_back(OutputIndex_Timestamp, std::vector{2}, DataType::DOUBLE); + outputPortsInfo.push_back( + {OutputIndex_Timestamp, Port::Dimensions{2}, Port::DataType::DOUBLE}); } if (!autoconnect) { // Use double anyway even if it is a bool signal. - ioData.output.emplace_back(OutputIndex_IsConnected, std::vector{1}, DataType::DOUBLE); + outputPortsInfo.push_back( + {OutputIndex_IsConnected, Port::Dimensions{1}, Port::DataType::DOUBLE}); } - if (!blockInfo->setIOPortsData(ioData)) { - wbtError << "Failed to configure input / output ports."; + if (!blockInfo->setPortsInfo({}, outputPortsInfo)) { + bfError << "Failed to configure input / output ports."; return false; } @@ -188,7 +192,7 @@ bool YarpRead::initialize(BlockInformation* blockInfo) // ========== if (!YarpRead::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -203,7 +207,7 @@ bool YarpRead::initialize(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("Timeout", pImpl->timeout); if (!ok) { - wbtError << "Failed to read input parameters."; + bfError << "Failed to read input parameters."; return false; } @@ -213,7 +217,7 @@ bool YarpRead::initialize(BlockInformation* blockInfo) Network::init(); if (!Network::initialized() || !Network::checkNetwork(5.0)) { - wbtError << "YARP server wasn't found active."; + bfError << "YARP server wasn't found active."; return false; } @@ -232,20 +236,20 @@ bool YarpRead::initialize(BlockInformation* blockInfo) } if (!pImpl->port.open(destinationPortName)) { - wbtError << "Error while opening yarp port."; + bfError << "Error while opening yarp port."; return false; } if (pImpl->autoconnect) { if (!Network::connect(pImpl->sourcePortName, pImpl->port.getName())) { if (pImpl->errorOnMissingPort) { - wbtError << "Failed to connect " + pImpl->sourcePortName + " to " - + pImpl->port.getName() + "."; + bfError << "Failed to connect " + pImpl->sourcePortName + " to " + + pImpl->port.getName() + "."; return false; } else { - wbtWarning << "Failed to connect " + pImpl->sourcePortName + " to " - + pImpl->port.getName() + "."; + bfWarning << "Failed to connect " + pImpl->sourcePortName + " to " + + pImpl->port.getName() + "."; } } } @@ -282,8 +286,8 @@ bool YarpRead::output(const BlockInformation* blockInfo) yarp::os::Time::delay(0.0005); const double now = yarp::os::SystemClock::nowSystem(); if ((now - t0) > pImpl->timeout) { - wbtError << "The port didn't receive any data for longer " - << "than the configured timeout."; + bfError << "The port didn't receive any data for longer " + << "than the configured timeout."; return false; } } @@ -297,14 +301,14 @@ bool YarpRead::output(const BlockInformation* blockInfo) if (pImpl->shouldReadTimestamp) { yarp::os::Stamp timestamp; if (!pImpl->port.getEnvelope(timestamp)) { - wbtError << "Failed to read port envelope (timestamp). Be sure" - << " that the input port actually writes this data."; + bfError << "Failed to read port envelope (timestamp). Be sure" + << " that the input port actually writes this data."; return false; } OutputSignalPtr timestampSignal = blockInfo->getOutputPortSignal(OutputIndex_Timestamp); if (!timestampSignal) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } @@ -315,19 +319,19 @@ bool YarpRead::output(const BlockInformation* blockInfo) OutputSignalPtr signal = blockInfo->getOutputPortSignal(OutputIndex::Signal); if (!signal) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } if (vectorBuffer->size() != signal->getWidth()) { - wbtError << "Size of received data from " << pImpl->port.getName() << " (" - << vectorBuffer->size() << ")" - << " does not match with output signal width (" << signal->getWidth() << ")."; + bfError << "Size of received data from " << pImpl->port.getName() << " (" + << vectorBuffer->size() << ")" + << " does not match with output signal width (" << signal->getWidth() << ")."; return false; } if (!signal->setBuffer(vectorBuffer->data(), vectorBuffer->size())) { - wbtError << "Failed to set the output buffer."; + bfError << "Failed to set the output buffer."; return false; } @@ -339,12 +343,12 @@ bool YarpRead::output(const BlockInformation* blockInfo) OutputSignalPtr statusPort = blockInfo->getOutputPortSignal(OutputIndex_IsConnected); if (!statusPort) { - wbtError << "Output signal not valid."; + bfError << "Output signal not valid."; return false; } if (!statusPort->set(0, 1)) { - wbtError << "Failed to write data to output buffer."; + bfError << "Failed to write data to output buffer."; return false; } } diff --git a/toolbox/library/src/YarpWrite.cpp b/toolbox/library/src/YarpWrite.cpp index 54c003177..371ed155b 100644 --- a/toolbox/library/src/YarpWrite.cpp +++ b/toolbox/library/src/YarpWrite.cpp @@ -6,13 +6,13 @@ * GNU Lesser General Public License v2.1 or any later version. */ -#include "YarpWrite.h" -#include "Core/BlockInformation.h" -#include "Core/Log.h" -#include "Core/Parameter.h" -#include "Core/Parameters.h" -#include "Core/Signal.h" +#include "WBToolbox/Block/YarpWrite.h" +#include +#include +#include +#include +#include #include #include #include @@ -21,7 +21,8 @@ #include #include -using namespace wbt; +using namespace wbt::block; +using namespace blockfactory::core; // INDICES: PARAMETERS, INPUTS, OUTPUT // =================================== @@ -75,7 +76,7 @@ bool YarpWrite::parseParameters(BlockInformation* blockInfo) for (const auto& md : metadata) { if (!blockInfo->addParameterMetadata(md)) { - wbtError << "Failed to store parameter metadata"; + bfError << "Failed to store parameter metadata"; return false; } } @@ -96,19 +97,17 @@ bool YarpWrite::configureSizeAndPorts(BlockInformation* blockInfo) // No outputs // - const bool ok = blockInfo->setIOPortsData({ + const bool ok = blockInfo->setPortsInfo( { // Inputs - std::make_tuple( - InputIndex::Signal, std::vector{Signal::DynamicSize}, DataType::DOUBLE), + {InputIndex::Signal, Port::Dimensions{Port::DynamicSize}, Port::DataType::DOUBLE}, }, { // Outputs - }, - }); + }); if (!ok) { - wbtError << "Failed to configure input / output ports."; + bfError << "Failed to configure input / output ports."; return false; } @@ -128,7 +127,7 @@ bool YarpWrite::initialize(BlockInformation* blockInfo) // ========== if (!YarpWrite::parseParameters(blockInfo)) { - wbtError << "Failed to parse parameters."; + bfError << "Failed to parse parameters."; return false; } @@ -140,7 +139,7 @@ bool YarpWrite::initialize(BlockInformation* blockInfo) ok = ok && m_parameters.getParameter("ErrorOnMissingPort", pImpl->errorOnMissingPort); if (!ok) { - wbtError << "Failed to read input parameters."; + bfError << "Failed to read input parameters."; return false; } @@ -150,7 +149,7 @@ bool YarpWrite::initialize(BlockInformation* blockInfo) Network::init(); if (!Network::initialized() || !Network::checkNetwork(5.0)) { - wbtError << "YARP server wasn't found active."; + bfError << "YARP server wasn't found active."; return false; } @@ -169,20 +168,20 @@ bool YarpWrite::initialize(BlockInformation* blockInfo) } if (!pImpl->port.open(sourcePortName)) { - wbtError << "Error while opening yarp port."; + bfError << "Error while opening yarp port."; return false; } if (pImpl->autoconnect) { if (!Network::connect(pImpl->port.getName(), pImpl->destinationPortName)) { if (pImpl->errorOnMissingPort) { - wbtError << "Failed to connect " << pImpl->port.getName() << " to " - << pImpl->destinationPortName << "."; + bfError << "Failed to connect " << pImpl->port.getName() << " to " + << pImpl->destinationPortName << "."; return false; } else { - wbtWarning << "Failed to connect " << pImpl->port.getName() << " to " - << pImpl->destinationPortName << "."; + bfWarning << "Failed to connect " << pImpl->port.getName() << " to " + << pImpl->destinationPortName << "."; } } } @@ -208,7 +207,7 @@ bool YarpWrite::output(const BlockInformation* blockInfo) InputSignalPtr signal = blockInfo->getInputPortSignal(InputIndex::Signal); if (!signal) { - wbtError << "Input signal not valid."; + bfError << "Input signal not valid."; return false; }