diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b99e7077..3cc6cbbd5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -3,20 +3,18 @@ # CopyPolicy: Released under the terms of the GNU GPL v2.0. cmake_minimum_required(VERSION 3.3) -project(WB-Toolbox) +project(WB-Toolbox LANGUAGES CXX VERSION 3.1) set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) +# Add custom functions / macros +list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake) + find_package(YCM REQUIRED) include(YCMDefaultDirs) -set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake) -set(WB-TOOLBOX_SHARE_DIR "${CMAKE_INSTALL_PREFIX}/share/WB-Toolbox") - add_subdirectory(MxAnyType) add_subdirectory(toolbox) -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/images DESTINATION ${WB-TOOLBOX_SHARE_DIR}) - include(AddUninstallTarget) diff --git a/MxAnyType/CMakeLists.txt b/MxAnyType/CMakeLists.txt index 608670096..cdf49b437 100644 --- a/MxAnyType/CMakeLists.txt +++ b/MxAnyType/CMakeLists.txt @@ -1,5 +1,6 @@ cmake_policy(SET CMP0048 NEW) project(MxAnyType LANGUAGES CXX VERSION 0.1) +# set (PROJECT_VERSION 0.1) # TODO cmake_minimum_required(VERSION 3.0.2) # Configure the project @@ -10,42 +11,61 @@ include(GNUInstallDirs) set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) # Use a general prefix for the project -set(VARS_PREFIX ${PROJECT_NAME}) -# set(VARS_PREFIX "MxAnyType") +# set(VARS_PREFIX ${PROJECT_NAME}) +set(VARS_PREFIX "MxAnyType") + +# TODO: when exporting this folder to a new repository, this will become +# a new project. Uncomment the project line and the alternative +# VARS_PREFIX. # Build the library # ================= -# Export the includes needed to who inherits this library -# Set this up properly for handling either BUILD and INSTALL trees -set(${VARS_PREFIX}_INCLUDE_BUILD ${CMAKE_CURRENT_SOURCE_DIR}/include) -set(${VARS_PREFIX}_INCLUDE_INSTALL ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX}) - # Add the target -add_library(${VARS_PREFIX} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/include/AnyType.h +add_library(${VARS_PREFIX} SHARED ${CMAKE_CURRENT_SOURCE_DIR}/include/AnyType.h ${CMAKE_CURRENT_SOURCE_DIR}/include/MxAnyType.h ${CMAKE_CURRENT_SOURCE_DIR}/src/MxAnyType.cpp) +set_target_properties(${VARS_PREFIX} PROPERTIES + VERSION ${PROJECT_VERSION} # This is for the symlink of the .so + PUBLIC_HEADER "include/AnyType.h;include/MxAnyType.h" +) # TODO: temporary, waiting the library becomes a shared -if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") - set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") -endif() +# if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") +# set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") +# endif() # Find Matlab resources find_package(Matlab REQUIRED MX_LIBRARY) -target_include_directories(${VARS_PREFIX} SYSTEM PUBLIC "${Matlab_INCLUDE_DIRS}") +target_include_directories(${VARS_PREFIX} SYSTEM PUBLIC ${Matlab_INCLUDE_DIRS}) # Setup the include directories -# TODO why in the how to was INTERFACE? target_include_directories(${VARS_PREFIX} PUBLIC - $ - $) - -# Assign some useful properties -# set_target_properties(${VARS_PREFIX} PROPERTIES VERSION ${PROJECT_VERSION} -# PUBLIC_HEADER MxAnyType.h) + $ + $) # Build tests # =========== + # add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/tests) + +# Install +# ======= + +install(TARGETS MxAnyType + #EXPORT WBToolboxSimulinkCoder + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX} +) + +# TODO: when splitting this library from the toolbox, this will provide find_package support +# include(InstallBasicPackageFiles) +# install_basic_package_files(WBToolboxSimulinkCoder +# VERSION ${PROJECT_VERSION} +# COMPATIBILITY AnyNewerVersion +# TARGETS_PROPERTY MxAnyType +# NO_CHECK_REQUIRED_COMPONENTS_MACRO +# ) diff --git a/MxAnyType/include/AnyType.h b/MxAnyType/include/AnyType.h index a13d1d03a..3a0cdeae6 100644 --- a/MxAnyType/include/AnyType.h +++ b/MxAnyType/include/AnyType.h @@ -1,9 +1,9 @@ #ifndef ANYTYPE_H #define ANYTYPE_H -#include -#include #include +#include +#include class AnyType; @@ -14,13 +14,12 @@ typedef std::unordered_map AnyStruct; class AnyType { protected: - public: AnyType() = default; virtual ~AnyType() = default; // Integers - virtual bool asInt(int& i) = 0; + 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; diff --git a/MxAnyType/include/MxAnyType.h b/MxAnyType/include/MxAnyType.h index b8990b741..8d236cc5a 100644 --- a/MxAnyType/include/MxAnyType.h +++ b/MxAnyType/include/MxAnyType.h @@ -1,12 +1,13 @@ #ifndef MXANYTYPE_H #define MXANYTYPE_H -#include +#include "AnyType.h" + +#include +#include #include #include -#include -#include "AnyType.h" -#include "matrix.h" +#include class MxAnyType; @@ -14,7 +15,8 @@ class MxAnyType; // class MxAnyCell : public AnyCell {}; // class MxAnyStruct : public AnyStruct {}; -struct MxArrayMetadata { +struct MxArrayMetadata +{ mxClassID id; bool isScalar; size_t rows; diff --git a/MxAnyType/src/MxAnyType.cpp b/MxAnyType/src/MxAnyType.cpp index 55e7fe61c..585d1bc85 100644 --- a/MxAnyType/src/MxAnyType.cpp +++ b/MxAnyType/src/MxAnyType.cpp @@ -1,4 +1,6 @@ #include "MxAnyType.h" + +#include #include // PRIVATE METHODS @@ -6,9 +8,17 @@ 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 + 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 @@ -18,10 +28,12 @@ bool MxAnyType::asScalar(double& d) bool MxAnyType::validateClassId(mxClassID id1, mxClassID id2) { - if (validate) + if (validate) { return id1 == id2; - else + } + else { return true; + } } // PUBLIC METHODS @@ -30,12 +42,16 @@ bool MxAnyType::validateClassId(mxClassID id1, mxClassID id2) // Constructors // ============ -MxAnyType::MxAnyType() : mx(nullptr), validate(false) +MxAnyType::MxAnyType() + : mx(nullptr) + , validate(false) { md.id = mxUNKNOWN_CLASS; } -MxAnyType::MxAnyType(const mxArray* m, bool validateId) : mx(m), validate(validateId) +MxAnyType::MxAnyType(const mxArray* m, bool validateId) + : mx(m) + , validate(validateId) { assert(mx); @@ -46,13 +62,14 @@ MxAnyType::MxAnyType(const mxArray* m, bool validateId) : mx(m), validate(valida // Get the other metadata md.isScalar = mxIsScalar(mx); - md.rows = static_cast(mxGetN(mx)); - md.cols = static_cast(mxGetM(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) + if (md.isScalar) { assert(md.rows == md.cols == md.nElem == 1); + } // TODO: only 2 dims currently supported assert(md.nDims == 2); @@ -66,9 +83,9 @@ MxAnyType::MxAnyType(const mxArray* m, bool validateId) : mx(m), validate(valida } MxAnyType::MxAnyType(const MxAnyType& mxAnyType) -: mx(mxAnyType.mx) -, md(mxAnyType.md) -, validate(mxAnyType.validate) + : mx(mxAnyType.mx) + , md(mxAnyType.md) + , validate(mxAnyType.validate) {} void MxAnyType::enableClassIDValidation() @@ -81,8 +98,12 @@ void MxAnyType::enableClassIDValidation() bool MxAnyType::asString(std::string& s) { - if (!mx) return false; - if (md.id != mxCHAR_CLASS) return false; + if (!mx) { + return false; + } + if (md.id != mxCHAR_CLASS) { + return false; + } char* buffer = mxArrayToString(mx); s = std::string(buffer); mxFree(buffer); @@ -98,7 +119,9 @@ bool MxAnyType::asString(std::string& s) bool MxAnyType::asInt(int& i) { double buffer; - if (!asScalar(buffer)) return false; + if (!asScalar(buffer)) { + return false; + } i = static_cast(buffer); return true; } @@ -106,7 +129,9 @@ bool MxAnyType::asInt(int& i) bool MxAnyType::asUInt(unsigned& i) { double buffer; - if (!asScalar(buffer)) return false; + if (!asScalar(buffer)) { + return false; + } i = static_cast(buffer); return true; } @@ -117,7 +142,9 @@ bool MxAnyType::asUInt(unsigned& i) bool MxAnyType::asInt32(int32_t& i) { double buffer; - if (!asScalar(buffer)) return false; + if (!asScalar(buffer)) { + return false; + } i = static_cast(buffer); return validateClassId(md.id, mxINT32_CLASS); } @@ -138,9 +165,13 @@ bool MxAnyType::asDouble(double& d) } bool MxAnyType::asBool(bool& b) - { - if (!mx) return false; - if (!mxIsLogicalScalar(mx)) return false; +{ + if (!mx) { + return false; + } + if (!mxIsLogicalScalar(mx)) { + return false; + } b = mxIsLogicalScalarTrue(mx); return true; } @@ -150,15 +181,23 @@ bool MxAnyType::asBool(bool& b) bool MxAnyType::asAnyStruct(AnyStruct& s) { - if (!mx) return false; - if (md.id != mxSTRUCT_CLASS) return false; + 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); + const char* fieldName = mxGetFieldNameByNumber(mx, i); // TODO multidimensional struct - mxArray* fieldContent = mxGetFieldByNumber(mx,0,i); - if (fieldName == nullptr) return false; - if (fieldContent == nullptr) return false; + mxArray* fieldContent = mxGetFieldByNumber(mx, 0, i); + if (!fieldName) { + return false; + } + if (!fieldContent) { + return false; + } s[std::string(fieldName)] = std::make_shared(fieldContent); } return true; @@ -166,13 +205,19 @@ bool MxAnyType::asAnyStruct(AnyStruct& s) bool MxAnyType::asAnyCell(AnyCell& cell) { - if (!mx) return false; - if (md.id != mxCELL_CLASS) return false; + 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) { + for (unsigned i = 0; i < mxGetNumberOfElements(mx); ++i) { mxArray* cellContent = mxGetCell(mx, i); - if (!cellContent) return false; + if (!cellContent) { + return false; + } cell.push_back(std::make_shared(cellContent)); } return true; @@ -185,21 +230,30 @@ bool MxAnyType::asAnyCell(AnyCell& cell) // ====== // TODO: -// Tests with other types (uint8) https://it.mathworks.com/help/matlab/apiref/mxgetdata.html (Description) +// 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 (!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; + if (mxIsComplex(mx)) { + return false; + } double* buffer = mxGetPr(mx); - if (!buffer) return false; + if (!buffer) { + return false; + } vec.reserve(md.rows * md.cols); vec.assign(buffer, buffer + md.rows * md.cols); diff --git a/toolbox/CMakeLists.txt b/toolbox/CMakeLists.txt index 1059ffe59..a477c3bdf 100755 --- a/toolbox/CMakeLists.txt +++ b/toolbox/CMakeLists.txt @@ -2,6 +2,9 @@ # Author: Jorhabib Eljaik, Francesco Romano, Diego Ferigo # CopyPolicy: Released under the terms of the GNU GPL v2.0. +# FIND DEPENDENCIES +# ================= + find_package(YARP REQUIRED) # Fail if YARP is not compiled as shared library @@ -10,21 +13,31 @@ 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(Matlab REQUIRED + MX_LIBRARY + ENG_LIBRARY + MAIN_PROGRAM +) + +find_package(iDynTree 0.7.2 REQUIRED) + +# This find loads Eigen from YCM. +# 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 +# ==================== + set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${YARP_MODULE_PATH}) include(YarpInstallationHelpers) yarp_configure_external_installation(codyco) -find_package(Matlab REQUIRED MX_LIBRARY - ENG_LIBRARY - MAIN_PROGRAM) - -if(NOT Matlab_FOUND) - message(FATAL_ERROR "Matlab not found") -endif() +# Add configure_block macro +include(Utilities) -# Settings for rpath +# Settings for RPATH if(NOT MSVC) - # Add the option to enable RPATH option(WB-TOOLBOX_ENABLE_RPATH "Enable RPATH installation" TRUE) mark_as_advanced(WB-TOOLBOX_ENABLE_RPATH) endif(NOT MSVC) @@ -35,35 +48,38 @@ add_install_rpath_support(BIN_DIRS ${CMAKE_INSTALL_PREFIX}/bin DEPENDS WB-TOOLBOX_ENABLE_RPATH USE_LINK_PATH) -find_package(Eigen3 REQUIRED) -find_package(iDynTree 0.7.2 REQUIRED) - -include(Utilities) +# TOOLBOX SHARED LIBRARY +# ====================== +# +# This library will be linked to the S-Function for the usage in Simulink, and to +# another implementation for its usage within the code generated by the Simulink Coder. -configure_block(BLOCK_NAME "Base" +configure_block( + BLOCK_NAME "Base" LIST_PREFIX WBT - SOURCES src/base/toolbox.cpp - src/base/Block.cpp + SOURCES src/base/Block.cpp src/base/BlockInformation.cpp - src/base/WBBlock.cpp - src/base/Log.cpp - src/base/ToolboxSingleton.cpp - src/base/factory.cpp - src/base/SimulinkBlockInformation.cpp - src/base/Signal.cpp src/base/Configuration.cpp + src/base/factory.cpp + src/base/Log.cpp + src/base/Parameter.cpp + src/base/Parameters.cpp src/base/RobotInterface.cpp - HEADERS include/base/toolbox.h - include/base/Block.h + src/base/Signal.cpp + src/base/ToolboxSingleton.cpp + src/base/WBBlock.cpp + HEADERS include/base/Block.h include/base/BlockInformation.h - include/base/WBBlock.h - include/base/Log.h - include/base/ToolboxSingleton.h - include/base/SimulinkBlockInformation.h - include/base/Signal.h include/base/Configuration.h + include/base/Log.h + include/base/Parameter.h + include/base/Parameters.h include/base/RobotInterface.h - ) + include/base/Signal.h + include/base/toolbox.h + include/base/ToolboxSingleton.h + include/base/WBBlock.h +) # configure_block(BLOCK_NAME "Inverse Kinematics" # GROUP "Model" @@ -72,15 +88,16 @@ configure_block(BLOCK_NAME "Base" # 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) - add_definitions(-DWBT_USES_ICUB) + # add_definitions(-DWBT_USES_ICUB) - configure_block(BLOCK_NAME "Minimum Jerk Trajectory Generator" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/MinimumJerkTrajectoryGenerator.cpp - HEADERS include/MinimumJerkTrajectoryGenerator.h) + # configure_block(BLOCK_NAME "Minimum Jerk Trajectory Generator" + # GROUP "Utilities" + # LIST_PREFIX WBT + # SOURCES src/MinimumJerkTrajectoryGenerator.cpp + # HEADERS include/MinimumJerkTrajectoryGenerator.h) configure_block(BLOCK_NAME "Discrete Filter" GROUP "Utilities" @@ -100,20 +117,20 @@ if (WBT_USES_ICUB) # include_directories(SYSTEM ${iKin_INCLUDE_DIRS}) # endif() - include_directories(SYSTEM ${ctrlLib_INCLUDE_DIRS}) + # include_directories(SYSTEM ${ctrlLib_INCLUDE_DIRS}) endif() -configure_block(BLOCK_NAME "Yarp Read" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/YarpRead.cpp - HEADERS include/YarpRead.h) - -configure_block(BLOCK_NAME "Yarp Write" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/YarpWrite.cpp - HEADERS include/YarpWrite.h) +# configure_block(BLOCK_NAME "Yarp Read" +# GROUP "Utilities" +# LIST_PREFIX WBT +# SOURCES src/YarpRead.cpp +# HEADERS include/YarpRead.h) +# +# configure_block(BLOCK_NAME "Yarp Write" +# GROUP "Utilities" +# LIST_PREFIX WBT +# SOURCES src/YarpWrite.cpp +# HEADERS include/YarpWrite.h) configure_block(BLOCK_NAME "Real Time Synchronizer" GROUP "Utilities" @@ -121,11 +138,11 @@ configure_block(BLOCK_NAME "Real Time Synchronizer" SOURCES src/RealTimeSynchronizer.cpp HEADERS include/RealTimeSynchronizer.h) -configure_block(BLOCK_NAME "Model Partitioner" - GROUP "Utilities" - LIST_PREFIX WBT - SOURCES src/ModelPartitioner.cpp - HEADERS include/ModelPartitioner.h) +# configure_block(BLOCK_NAME "Model Partitioner" +# GROUP "Utilities" +# LIST_PREFIX WBT +# SOURCES src/ModelPartitioner.cpp +# HEADERS include/ModelPartitioner.h) configure_block(BLOCK_NAME "Yarp Clock" GROUP "Utilities" @@ -138,18 +155,19 @@ configure_block(BLOCK_NAME "Simulator Synchronizer" LIST_PREFIX WBT SOURCES src/SimulatorSynchronizer.cpp HEADERS include/SimulatorSynchronizer.h) +add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/autogenerated) -configure_block(BLOCK_NAME "Mass Matrix" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/MassMatrix.cpp - HEADERS include/MassMatrix.h) - -configure_block(BLOCK_NAME "Inverse Dynamics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/InverseDynamics.cpp - HEADERS include/InverseDynamics.h) +# configure_block(BLOCK_NAME "Mass Matrix" +# GROUP "Model" +# LIST_PREFIX WBT +# SOURCES src/MassMatrix.cpp +# HEADERS include/MassMatrix.h) +# +# configure_block(BLOCK_NAME "Inverse Dynamics" +# GROUP "Model" +# LIST_PREFIX WBT +# SOURCES src/InverseDynamics.cpp +# HEADERS include/InverseDynamics.h) configure_block(BLOCK_NAME "Centroidal Momentum" GROUP "Model" @@ -157,17 +175,17 @@ configure_block(BLOCK_NAME "Centroidal Momentum" SOURCES src/CentroidalMomentum.cpp HEADERS include/CentroidalMomentum.h) -configure_block(BLOCK_NAME "Forward Kinematics" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/ForwardKinematics.cpp - HEADERS include/ForwardKinematics.h) - -configure_block(BLOCK_NAME "Jacobian" - GROUP "Model" - LIST_PREFIX WBT - SOURCES src/Jacobian.cpp - HEADERS include/Jacobian.h) +# configure_block(BLOCK_NAME "Forward Kinematics" +# GROUP "Model" +# LIST_PREFIX WBT +# SOURCES src/ForwardKinematics.cpp +# HEADERS include/ForwardKinematics.h) +# +# configure_block(BLOCK_NAME "Jacobian" +# GROUP "Model" +# LIST_PREFIX WBT +# SOURCES src/Jacobian.cpp +# HEADERS include/Jacobian.h) configure_block(BLOCK_NAME "DotJ Nu" GROUP "Model" @@ -180,79 +198,156 @@ configure_block(BLOCK_NAME "Set References" LIST_PREFIX WBT SOURCES src/SetReferences.cpp HEADERS include/SetReferences.h) - -configure_block(BLOCK_NAME "Set Low-Level PIDs" - GROUP "Actuators" - LIST_PREFIX WBT - SOURCES src/SetLowLevelPID.cpp - HEADERS include/SetLowLevelPID.h) # +# configure_block(BLOCK_NAME "Set Low-Level PIDs" +# GROUP "Actuators" +# LIST_PREFIX WBT +# SOURCES src/SetLowLevelPID.cpp +# HEADERS include/SetLowLevelPID.h) + configure_block(BLOCK_NAME "Get Measurement" GROUP "State" LIST_PREFIX WBT SOURCES src/GetMeasurement.cpp HEADERS include/GetMeasurement.h) -configure_block(BLOCK_NAME "Get Limits" - GROUP "State" - LIST_PREFIX WBT - SOURCES src/GetLimits.cpp - HEADERS include/GetLimits.h) +# configure_block(BLOCK_NAME "Get Limits" +# GROUP "State" +# LIST_PREFIX WBT +# SOURCES src/GetLimits.cpp +# HEADERS include/GetLimits.h) + +# configure_block(BLOCK_NAME "Test New Configuration" +# GROUP "Utilities" +# LIST_PREFIX WBT +# SOURCES src/TestNewConf.cpp +# HEADERS include/TestNewConf.h) get_property(ALL_HEADERS GLOBAL PROPERTY WBT_HEADERS) get_property(ALL_SOURCES GLOBAL PROPERTY WBT_SOURCES) -add_definitions(-DMATLAB_MEX_FILE) -include_directories(include) -include_directories(include/base) +add_library(WBToolboxLibrary SHARED + ${ALL_HEADERS} ${ALL_SOURCES} +) +set_target_properties(WBToolboxLibrary PROPERTIES + OUTPUT_NAME "WBToolbox" + # VERSION ${PROJECT_VERSION} + PUBLIC_HEADER "${ALL_HEADERS}" +) + +# Set other properties of the target WBToolbox +# -------------------------------------------- + +target_include_directories(WBToolboxLibrary PUBLIC + $ + $ +) -include_directories(SYSTEM ${Matlab_INCLUDE_DIRS} "${Matlab_ROOT_DIR}/simulink/include") -include_directories(SYSTEM ${YARP_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIR}) +target_include_directories(WBToolboxLibrary SYSTEM PUBLIC + ${Matlab_INCLUDE_DIRS} + ${Matlab_ROOT_DIR}/simulink/include + ${YARP_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIR} +) -list(APPEND LINKED_LIBRARIES ${YARP_LIBRARIES}) -list(APPEND LINKED_LIBRARIES ${iDynTree_LIBRARIES}) +set(IDYNTREE_REQUIRED_LIBS + iDynTree::idyntree-core + iDynTree::idyntree-model + iDynTree::idyntree-modelio-urdf + iDynTree::idyntree-high-level +) +target_link_libraries(WBToolboxLibrary PUBLIC + ${YARP_LIBRARIES} + ${IDYNTREE_REQUIRED_LIBS} + ClockServer +) if (WBT_USES_ICUB) - list(APPEND LINKED_LIBRARIES ctrlLib) + target_compile_definitions(WBToolboxLibrary PUBLIC -DWBT_USES_ICUB) + target_link_libraries(WBToolboxLibrary PUBLIC ctrlLib) + #target_include_directories(WBToolboxLibrary PUBLIC ${ctrlLib_INCLUDE_DIRS}) if (${ICUB_USE_IPOPT}) - list(APPEND LINKED_LIBRARIES iKin) + target_link_libraries(WBToolboxLibrary PUBLIC iKin) endif() endif() -# Adding files used for the generation of the dynamic library +# TOOLBOX S-FUNCTION MEX LIBRARY +# ============================== + matlab_add_mex( - NAME WBToolbox - SRC ${ALL_HEADERS} ${ALL_SOURCES} - LINK_TO ${LINKED_LIBRARIES} + NAME WBToolboxMex + OUTPUT_NAME WBToolbox + SRC include/base/SimulinkBlockInformation.h + src/base/SimulinkBlockInformation.cpp + src/base/WBToolbox.cpp + LINK_TO MxAnyType WBToolboxLibrary ) -# Link with MxAnyType library -# TODO: this will become an external project sooner or later -target_link_libraries(WBToolbox MxAnyType) +target_compile_options(WBToolboxMex PUBLIC -DMATLAB_MEX_FILE) -# Link with ClockServer library -add_subdirectory(autogenerated/) -target_link_libraries(WBToolbox ClockServer) +install(TARGETS WBToolboxMex DESTINATION ${CMAKE_INSTALL_PREFIX}/mex) -install(TARGETS WBToolbox DESTINATION ${CMAKE_INSTALL_PREFIX}/mex) +# MATLAB CLASSES AND SIMULINK CONFIGURATION +# ========================================= -# The following line is to properly configure the installation script of the toolbox -configure_file(${CMAKE_CURRENT_SOURCE_DIR}/matlab/startup_wbitoolbox.m.in - ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m) +# Export the library +# ------------------ + +# The logic about library development is the following: +# +# * Developers should edit the library (e.g. changing block's masks) in the +# toolbox/matlab/library/WBToolboxLibrary_repository.mdl file. +# * Users will use the files in the exported/ folder, and the custom target +# export_libraries is used to export the WBToolboxLibrary_repository.mdl +# to the supported Matlab versions. -# Custom script to generate the library to be committed on the repository +# Custom script to generate the library to be committed on the repository. # This target is excluded from the normal build and must be called explicitly by the -# developer who modifies the library -add_custom_target(export_libraries ${Matlab_MAIN_PROGRAM} -nosplash -nodesktop -r export_library WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab) +# developer who modifies the library. +add_custom_target(export_libraries + ${Matlab_MAIN_PROGRAM} -nosplash -nodesktop -r export_library + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab +) set_target_properties(export_libraries PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD 1) +# Install all the other files needed by the toolbox +# ------------------------------------------------- + +set(WB-TOOLBOX_SHARE_DIR "${CMAKE_INSTALL_PREFIX}/share/WB-Toolbox") + +# The following line is to properly configure the installation script of the toolbox +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/matlab/startup_wbitoolbox.m.in + ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m) + # Install configuration files -install(FILES ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) -install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/slblocks.m DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +install( + FILES ${CMAKE_BINARY_DIR}/startup_wbitoolbox.m + DESTINATION ${WB-TOOLBOX_SHARE_DIR} +) +install( + FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/slblocks.m + DESTINATION ${WB-TOOLBOX_SHARE_DIR} +) + +# Install the Matlab package folder +install( + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab/+WBToolbox + DESTINATION ${WB-TOOLBOX_SHARE_DIR} +) + +# Install images for the Simulink Library +install( + DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/images + DESTINATION ${WB-TOOLBOX_SHARE_DIR} +) -# Install the package folder -install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab/+WBToolbox - DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +# Install script for enabling debugging capabilities +# (update the mex and avoid restarting matlab) +configure_file( + matlab/setupForMatlabDebug.m.in + ${CMAKE_BINARY_DIR}/toolbox/setupForMatlabDebug.m + @ONLY +) #if MAJOR >= 2014 && MINOR >= b # Note: We had issues with Matlab 2014b and .mdl models. @@ -263,5 +358,77 @@ install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/matlab/+WBToolbox # else() # install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/WBToolboxLibrary.mdl DESTINATION ${WB-TOOLBOX_SHARE_DIR}) # endif() -install(FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/library/exported/WBToolboxLibrary.slx - DESTINATION ${WB-TOOLBOX_SHARE_DIR}) +install( + FILES ${CMAKE_CURRENT_SOURCE_DIR}/matlab/library/exported/WBToolboxLibrary.slx + DESTINATION ${WB-TOOLBOX_SHARE_DIR} +) + +# AUTOMATIC CODE GENERATION SUPPORT +# ================================= + +set(CODER_HDR + include/base/CoderBlockInformation.h + include/base/GeneratedCodeWrapper.h +) + +set(CODER_SRC + src/base/CoderBlockInformation.cpp +) + +add_library(WBToolboxCoder SHARED ${CODER_HDR} ${CODER_SRC} + #src/base/GeneratedCodeWrapper.cpp +) + +target_link_libraries(WBToolboxCoder PUBLIC WBToolboxLibrary) + +set_target_properties(WBToolboxCoder PROPERTIES + # VERSION ${PROJECT_VERSION} + PUBLIC_HEADER "${CODER_HDR}" +) + +target_include_directories(WBToolboxCoder INTERFACE + $) + +# INSTALL THE WBToolboxCoder TARGET +# ================================= +# +# This target will be imported and linked with the code automatically +# generated by Simulink Coder. + +install( + TARGETS WBToolboxCoder WBToolboxLibrary + EXPORT WBToolboxSimulinkCoder + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/WB-Toolbox +) + +set_property(GLOBAL APPEND + PROPERTY WBToolboxSimulinkCoderExport_TARGETS + WBToolboxCoder WBToolboxLibrary) + +set(WBToolboxSimulinkCoderExport_DEPENDENCIES YARP iDynTree) +if (WBT_USES_ICUB) + list(APPEND WBToolboxSimulinkCoderExport_DEPENDENCIES iCub) +endif() + +include(InstallBasicPackageFiles) +install_basic_package_files(WBToolboxSimulinkCoder + VERSION ${PROJECT_VERSION} + DEPENDENCIES ${WBToolboxSimulinkCoderExport_DEPENDENCIES} + COMPATIBILITY AnyNewerVersion + TARGETS_PROPERTY WBToolboxSimulinkCoderExport_TARGETS + NO_CHECK_REQUIRED_COMPONENTS_MACRO +) + +# TODO: The current structure of this CMake file creates and installs two different libraries: +# libWBToolbox.so and libWBToolboxCoder.so. +# The first one is a generic library that can be linked with either Simulink or +# Simulink Coder headers. However, the usage of the WBToolbox in the first case +# (Simulink) is demanded to the mex library. It is pointless at the current state +# exporting the libWBToolbox.so. +# It would be nice being able to merge these two libraries together into a bigger +# libWBToolboxCoder.so, and in theory this is possible exploiting CMake's OBJECT. +# However, limitations in exporting (supported from CMake 3.9) and installing +# such type of targets prevent this usage. diff --git a/toolbox/autogenerated/CMakeLists.txt b/toolbox/autogenerated/CMakeLists.txt index 3ce96c1c5..51259d631 100644 --- a/toolbox/autogenerated/CMakeLists.txt +++ b/toolbox/autogenerated/CMakeLists.txt @@ -1,8 +1,8 @@ -cmake_policy(SET CMP0048 NEW) -project(ClockServer LANGUAGES CXX VERSION 0.1) -cmake_minimum_required(VERSION 3.0.2) +#cmake_policy(SET CMP0048 NEW) +#project(ClockServer LANGUAGES CXX VERSION 0.1) +#cmake_minimum_required(VERSION 3.0.2) -# Configure the project +# CONFIGURE THE PROJECT # ===================== set(CMAKE_CXX_STANDARD 11) set(CMAKE_CXX_STANDARD_REQUIRED ON) @@ -10,29 +10,47 @@ include(GNUInstallDirs) set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) # Use a general prefix for the project -set(VARS_PREFIX ${PROJECT_NAME}) +#set(VARS_PREFIX ${PROJECT_NAME}) +#set(VARS_PREFIX ClockServer) -# Build the library +# BUILD THE LIBRARY # ================= # Export the includes needed to who inherits this library # Set this up properly for handling either BUILD and INSTALL trees -set(${VARS_PREFIX}_INCLUDE_BUILD ${CMAKE_CURRENT_SOURCE_DIR}/include) -set(${VARS_PREFIX}_INCLUDE_INSTALL ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX}) +#set(${VARS_PREFIX}_INCLUDE_BUILD ${CMAKE_CURRENT_SOURCE_DIR}/include) +#set(${VARS_PREFIX}_INCLUDE_INSTALL ${CMAKE_INSTALL_INCLUDEDIR}/${VARS_PREFIX}) # Add the target -add_library(${VARS_PREFIX} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/include/thrift/ClockServer.h - ${CMAKE_CURRENT_SOURCE_DIR}/src/thrift/ClockServer.cpp) +add_library(ClockServer STATIC ${CMAKE_CURRENT_SOURCE_DIR}/include/thrift/ClockServer.h + ${CMAKE_CURRENT_SOURCE_DIR}/src/thrift/ClockServer.cpp) -if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") - set_target_properties(${VARS_PREFIX} PROPERTIES COMPILE_FLAGS "-fPIC") -endif() +# Needed for static libraries +set_target_properties(ClockServer PROPERTIES POSITION_INDEPENDENT_CODE ON) +#if (CMAKE_SYSTEM_PROCESSOR STREQUAL "x86_64") +# set_target_properties(ClockServer PROPERTIES COMPILE_FLAGS "-fPIC") +#endif() -# Setup the include directories -target_include_directories(${VARS_PREFIX} PUBLIC - $ - $) +#set_target_properties(ClockServer PROPERTIES +# #VERSION ${PROJECT_VERSION} # This is for the symlink of the .so +# PUBLIC_HEADER ${CMAKE_CURRENT_SOURCE_DIR}/include/thrift/ClockServer.h +#) # YARP find_package(YARP REQUIRED) -target_include_directories(${VARS_PREFIX} PUBLIC ${YARP_INCLUDE_DIRS}) + +# Setup the include directories +target_include_directories(ClockServer PUBLIC + ${YARP_INCLUDE_DIRS} + $) + #$) + +install(TARGETS ClockServer + EXPORT WBToolboxSimulinkCoder + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/ClockServer +) + +set_property(GLOBAL APPEND PROPERTY WBToolboxSimulinkCoderExport_TARGETS ClockServer) diff --git a/images/Dynamics.png b/toolbox/images/Dynamics.png similarity index 100% rename from images/Dynamics.png rename to toolbox/images/Dynamics.png diff --git a/images/forwardKinematics.png b/toolbox/images/forwardKinematics.png similarity index 100% rename from images/forwardKinematics.png rename to toolbox/images/forwardKinematics.png diff --git a/images/jacobian.png b/toolbox/images/jacobian.png similarity index 100% rename from images/jacobian.png rename to toolbox/images/jacobian.png diff --git a/images/utilities.png b/toolbox/images/utilities.png similarity index 100% rename from images/utilities.png rename to toolbox/images/utilities.png diff --git a/images/wholeBodyActuators.png b/toolbox/images/wholeBodyActuators.png similarity index 100% rename from images/wholeBodyActuators.png rename to toolbox/images/wholeBodyActuators.png diff --git a/images/wholeBodyModel.png b/toolbox/images/wholeBodyModel.png similarity index 100% rename from images/wholeBodyModel.png rename to toolbox/images/wholeBodyModel.png diff --git a/images/wholeBodyStates.png b/toolbox/images/wholeBodyStates.png similarity index 100% rename from images/wholeBodyStates.png rename to toolbox/images/wholeBodyStates.png diff --git a/toolbox/include/CentroidalMomentum.h b/toolbox/include/CentroidalMomentum.h index bfb620a60..3c2c3d133 100644 --- a/toolbox/include/CentroidalMomentum.h +++ b/toolbox/include/CentroidalMomentum.h @@ -17,12 +17,6 @@ class wbt::CentroidalMomentum : public wbt::WBBlock private: std::unique_ptr m_centroidalMomentum; - static const unsigned INPUT_IDX_BASE_POSE; - static const unsigned INPUT_IDX_JOINTCONF; - static const unsigned INPUT_IDX_BASE_VEL; - static const unsigned INPUT_IDX_JOINT_VEL; - static const unsigned OUTPUT_IDX_CENTRMOM; - public: static const std::string ClassName; @@ -30,8 +24,7 @@ class wbt::CentroidalMomentum : public wbt::WBBlock ~CentroidalMomentum() override = default; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; bool output(const BlockInformation* blockInfo) override; }; diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h index fead433f7..601e37d62 100644 --- a/toolbox/include/DiscreteFilter.h +++ b/toolbox/include/DiscreteFilter.h @@ -1,4 +1,5 @@ #include "Block.h" +#include "Parameters.h" #include #include #include @@ -25,23 +26,11 @@ namespace yarp { class wbt::DiscreteFilter : public wbt::Block { private: - unsigned inputSignalWidth; - std::unique_ptr filter; - std::unique_ptr y0; - std::unique_ptr u0; - std::unique_ptr inputSignalVector; + std::unique_ptr m_filter; + std::unique_ptr m_y0; + std::unique_ptr m_u0; + std::unique_ptr m_inputSignalVector; - static void stringToYarpVector(const std::string s, yarp::sig::Vector& v); - - // Parameters - static const unsigned PARAM_IDX_FLT_TYPE; - static const unsigned PARAM_IDX_NUMCOEFF; - static const unsigned PARAM_IDX_DENCOEFF; - static const unsigned PARAM_IDX_1LOWP_FC; - static const unsigned PARAM_IDX_1LOWP_TS; - static const unsigned PARAM_IDX_MD_ORDER; - static const unsigned PARAM_IDX_INIT_Y0; - static const unsigned PARAM_IDX_INIT_U0; // Inputs static const unsigned INPUT_IDX_SIGNAL; // Outputs @@ -56,8 +45,9 @@ class wbt::DiscreteFilter : public wbt::Block ~DiscreteFilter() override = default; unsigned numberOfParameters() override; + bool parseParameters(BlockInformation* blockInfo) override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(const 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; diff --git a/toolbox/include/DotJNu.h b/toolbox/include/DotJNu.h index 7ae651c56..026d53902 100644 --- a/toolbox/include/DotJNu.h +++ b/toolbox/include/DotJNu.h @@ -20,12 +20,6 @@ class wbt::DotJNu : public wbt::WBBlock bool m_frameIsCoM; iDynTree::FrameIndex m_frameIndex; - static const unsigned INPUT_IDX_BASE_POSE; - static const unsigned INPUT_IDX_JOINTCONF; - static const unsigned INPUT_IDX_BASE_VEL; - static const unsigned INPUT_IDX_JOINT_VEL; - static const unsigned OUTPUT_IDX_DOTJ_NU; - public: static const std::string ClassName; @@ -34,8 +28,9 @@ class wbt::DotJNu : public wbt::WBBlock unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; + bool parseParameters(BlockInformation* blockInfo) override; - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; bool output(const BlockInformation* blockInfo) override; }; diff --git a/toolbox/include/GetLimits.h b/toolbox/include/GetLimits.h index 98e9901f6..410895002 100644 --- a/toolbox/include/GetLimits.h +++ b/toolbox/include/GetLimits.h @@ -33,9 +33,10 @@ class wbt::GetLimits : public wbt::WBBlock ~GetLimits() override = default; unsigned numberOfParameters() override; + bool parseParameters(BlockInformation* blockInfo) override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; bool output(const BlockInformation* blockInfo) override; }; diff --git a/toolbox/include/GetMeasurement.h b/toolbox/include/GetMeasurement.h index 5712f481e..9287d94e0 100644 --- a/toolbox/include/GetMeasurement.h +++ b/toolbox/include/GetMeasurement.h @@ -28,9 +28,9 @@ class wbt::GetMeasurement : public wbt::WBBlock ~GetMeasurement() override = default; unsigned numberOfParameters() override; + bool parseParameters(BlockInformation* blockInfo) override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; bool output(const BlockInformation* blockInfo) override; }; diff --git a/toolbox/include/RealTimeSynchronizer.h b/toolbox/include/RealTimeSynchronizer.h index 26a797589..7cca8e10e 100644 --- a/toolbox/include/RealTimeSynchronizer.h +++ b/toolbox/include/RealTimeSynchronizer.h @@ -16,18 +16,16 @@ class wbt::RealTimeSynchronizer : public wbt::Block ~RealTimeSynchronizer() override = default; unsigned numberOfParameters() override; + bool parseParameters(BlockInformation* blockInfo) override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; bool output(const BlockInformation* blockInfo) override; private: double m_period; - double m_initialTime; unsigned long m_counter; - - static const unsigned PARAM_PERIOD; // Period }; #endif /* end of include guard: WBT_REALTIMESYNCHRONIZER_H */ diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index 16e2eba7a..9ddf0532d 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -23,9 +23,9 @@ class wbt::SetReferences : public wbt::WBBlock ~SetReferences() override = default; unsigned numberOfParameters() override; + bool parseParameters(BlockInformation* blockInfo) override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - - bool initialize(const 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; diff --git a/toolbox/include/SimulatorSynchronizer.h b/toolbox/include/SimulatorSynchronizer.h index 42fc3750d..9c0bca9b7 100644 --- a/toolbox/include/SimulatorSynchronizer.h +++ b/toolbox/include/SimulatorSynchronizer.h @@ -18,8 +18,9 @@ class wbt::SimulatorSynchronizer : public wbt::Block unsigned numberOfParameters() override; std::vector additionalBlockOptions() override; + bool parseParameters(BlockInformation* blockInfo) override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; bool output(const BlockInformation* blockInfo) override; @@ -29,10 +30,6 @@ class wbt::SimulatorSynchronizer : public wbt::Block struct RPCData; std::unique_ptr m_rpcData; - - static const unsigned PARAM_PERIOD; // Period - static const unsigned PARAM_GZCLK_PORT; // Gazebo clock port - static const unsigned PARAM_RPC_PORT; // RPC client port name }; #endif /* end of include guard: WBT_SIMULATORSYNCHRONIZER_H */ diff --git a/toolbox/include/YarpClock.h b/toolbox/include/YarpClock.h index 430c78838..dcdcabaee 100644 --- a/toolbox/include/YarpClock.h +++ b/toolbox/include/YarpClock.h @@ -17,7 +17,7 @@ class wbt::YarpClock : public wbt::Block unsigned numberOfParameters() override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; bool output(const BlockInformation* blockInfo) override; }; diff --git a/toolbox/include/YarpWrite.h b/toolbox/include/YarpWrite.h index 206567dff..a5b4c9325 100644 --- a/toolbox/include/YarpWrite.h +++ b/toolbox/include/YarpWrite.h @@ -16,7 +16,7 @@ namespace yarp { namespace sig { class Vector; } -} +} // namespace yarp class wbt::YarpWrite : public wbt::Block { @@ -39,7 +39,7 @@ class wbt::YarpWrite : public wbt::Block std::string m_destinationPortName; std::unique_ptr> m_port; - static const unsigned PARAM_IDX_PORTNAME; // Port name + static const unsigned PARAM_IDX_PORTNAME; // Port name static const unsigned PARAM_IDX_AUTOCONNECT; // Autoconnect boolean static const unsigned PARAM_IDX_ERR_NO_PORT; // Error on missing port if autoconnect is true }; diff --git a/toolbox/include/base/Block.h b/toolbox/include/base/Block.h index 0e0667d27..90e666508 100644 --- a/toolbox/include/base/Block.h +++ b/toolbox/include/base/Block.h @@ -1,13 +1,32 @@ +// Copyright (c) 2017 iCub Facility - Istituto Italiano di Tecnologia +// +// Author: Diego Ferigo +// Author: Francesco Romano +// Author: Jorhabib Eljaik Gomez +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . + #ifndef WBT_BLOCK_H #define WBT_BLOCK_H +#include "Parameters.h" #include #include namespace wbt { class Block; class BlockInformation; - } // namespace wbt /** @@ -23,6 +42,9 @@ namespace wbt { */ class wbt::Block { +protected: + Parameters parameters; + public: /** * Create and returns a new Block object of the specified class. @@ -38,14 +60,21 @@ class wbt::Block * Destructor * */ - virtual ~Block(); + virtual ~Block() = default; + + /** + * @brief Static variable matching Block::numberOfParameters(). + * It might be useful to define parametric constants for indices + * in child blocks. + */ + static const unsigned NumberOfParameters; /** * Returns the number of configuration parameters needed by this block * * @return the number of parameters */ - virtual unsigned numberOfParameters() = 0; + virtual unsigned numberOfParameters(); /** * Returns vector of additional block options @@ -101,6 +130,9 @@ class wbt::Block */ virtual void parameterAtIndexIsTunable(unsigned index, bool& tunable); + // TODO + virtual bool parseParameters(BlockInformation* blockInfo); + /** * Configure the input and output ports * @@ -121,6 +153,7 @@ class wbt::Block * * @return true for success, false otherwise */ + // TODO: what is this?? virtual bool checkParameters(const BlockInformation* blockInfo); /** @@ -132,7 +165,7 @@ class wbt::Block * * @return true for success, false otherwise */ - virtual bool initialize(const BlockInformation* blockInfo) = 0; + virtual bool initialize(BlockInformation* blockInfo) = 0; /** * Called to initialize the initial conditions @@ -168,6 +201,9 @@ class wbt::Block */ virtual bool output(const BlockInformation* blockInfo) = 0; + // TODO + virtual bool getParameters(wbt::Parameters& params) const; + public: }; diff --git a/toolbox/include/base/BlockInformation.h b/toolbox/include/base/BlockInformation.h index 232cedff0..49a819646 100644 --- a/toolbox/include/base/BlockInformation.h +++ b/toolbox/include/base/BlockInformation.h @@ -1,12 +1,16 @@ #ifndef WBT_BLOCKINFORMATION_H #define WBT_BLOCKINFORMATION_H -#include "AnyType.h" +#include #include namespace wbt { class BlockInformation; class Signal; + class ParameterMetadata; + class Parameters; + class Configuration; + class RobotInterface; typedef enum _PortDataType { @@ -24,9 +28,12 @@ namespace wbt { extern const std::string BlockOptionPrioritizeOrder; } // namespace wbt +namespace iDynTree { + class KinDynComputations; +} + class wbt::BlockInformation { - public: BlockInformation() = default; virtual ~BlockInformation() = default; @@ -46,20 +53,8 @@ class wbt::BlockInformation // PARAMETERS METHODS // ================== - /** - * Reads the parameter at the specified index and interpret it as a string - * - * @param parameterIndex index of the parameter to be read - * @param [out]stringParameter resulting parameter - * - * @return true if success, false otherwise - */ - virtual bool getStringParameterAtIndex(unsigned parameterIndex, - std::string& stringParameter) const = 0; - virtual bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const = 0; - virtual bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const = 0; - virtual bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const = 0; - virtual bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const = 0; + virtual bool parseParameters(wbt::Parameters& parameters) { return true; }; + virtual bool addParameterMetadata(const wbt::ParameterMetadata& paramMD) { return true; }; // PORT INFORMATION SETTERS // ======================== @@ -89,6 +84,15 @@ class wbt::BlockInformation virtual unsigned getOutputPortWidth(unsigned portNumber) const = 0; virtual wbt::Signal getInputPortSignal(unsigned portNumber, int portWidth = -1) const = 0; virtual wbt::Signal getOutputPortSignal(unsigned portNumber, int portWidth = -1) const = 0; + + // TODO + // HANDLE ROBOT RESOURCES + // ====================== + + // This virtual method is temporary for 3.1. In the future YARP stuff will be + // abstracted from the toolbox. iDynTree instead will stay. + virtual std::weak_ptr getRobotInterface() const = 0; + virtual std::weak_ptr getKinDynComputations() const = 0; }; #endif /* end of include guard: WBT_BLOCKINFORMATION_H */ diff --git a/toolbox/include/base/CoderBlockInformation.h b/toolbox/include/base/CoderBlockInformation.h new file mode 100644 index 000000000..a3b148f5f --- /dev/null +++ b/toolbox/include/base/CoderBlockInformation.h @@ -0,0 +1,80 @@ +#ifndef WBT_CODERBLOCKINFORMATION_H +#define WBT_CODERBLOCKINFORMATION_H + +#include "BlockInformation.h" +#include "Parameters.h" +#include "Signal.h" + +#include +#include + +namespace wbt { + class CoderBlockInformation; +} + +class wbt::CoderBlockInformation : public wbt::BlockInformation +{ +private: + typedef int Rows; + typedef int Cols; + typedef unsigned SignalIndex; + + std::vector m_paramsMetadata; + std::unordered_map m_inputSignals; + std::unordered_map m_outputSignals; + + std::string m_confBlockName; + Parameters m_parametersFromRTW; + + std::unordered_map> m_inputSignalSize; + std::unordered_map> m_outputSignalSize; + + // Private? Static? + // TODO + static bool containConfigurationData(const wbt::Parameters& parameters); + +public: + CoderBlockInformation() = default; + ~CoderBlockInformation() 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 setNumberOfInputPorts(unsigned numberOfPorts) override; + bool setNumberOfOutputPorts(unsigned numberOfPorts) override; + bool setInputPortVectorSize(unsigned portNumber, int portSize) override; + bool setInputPortMatrixSize(unsigned portNumber, int rows, int columns) override; + bool setOutputPortVectorSize(unsigned portNumber, int portSize) override; + bool setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) override; + bool setInputPortType(unsigned portNumber, PortDataType portType) override; + bool setOutputPortType(unsigned portNumber, PortDataType portType) override; + + // PORT INFORMATION GETTERS + // ======================== + + unsigned getInputPortWidth(unsigned portNumber) const override; + unsigned getOutputPortWidth(unsigned portNumber) const override; + wbt::Signal getInputPortSignal(unsigned portNumber, int portWidth = -1) const override; + wbt::Signal getOutputPortSignal(unsigned portNumber, int portWidth = -1) const override; + + std::weak_ptr getRobotInterface() const override; + std::weak_ptr getKinDynComputations() const override; + + // Methods outside the interface + bool storeRTWParameters(const Parameters& stringParams); + bool setInputSignal(unsigned portNumber, void* address, int portWidth); + bool setOutputSignal(unsigned portNumber, void* address, int portWidth); +}; + +#endif /* end of include guard: WBT_CODERBLOCKINFORMATION_H */ diff --git a/toolbox/include/base/Configuration.h b/toolbox/include/base/Configuration.h index 780cdcc4e..c2078d253 100644 --- a/toolbox/include/base/Configuration.h +++ b/toolbox/include/base/Configuration.h @@ -20,7 +20,7 @@ namespace wbt { class wbt::Configuration { private: - const std::string m_confKey; ///< Name of the block which this object refers to + std::string m_confKey; ///< Name of the block which this object refers to std::string m_robotName; ///< Name of the robot std::string m_urdfFile; ///< Name of the file containing the urdf model std::string m_localName; ///< Prefix appended to the opened ports @@ -30,7 +30,7 @@ class wbt::Configuration size_t m_dofs; //< DoFs extracted my m_controlBoardsNames vector public: - Configuration() = delete; + Configuration() = default; Configuration(const std::string& confKey); ~Configuration() = default; @@ -154,6 +154,9 @@ class wbt::Configuration */ const size_t& getNumberOfDoFs() const; + // TODO + const std::string getConfKey() const { return m_confKey; } + // OTHER METHODS // ============= diff --git a/toolbox/include/base/GeneratedCodeWrapper.h b/toolbox/include/base/GeneratedCodeWrapper.h new file mode 100644 index 000000000..0f491f815 --- /dev/null +++ b/toolbox/include/base/GeneratedCodeWrapper.h @@ -0,0 +1,123 @@ +#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/include/base/Parameter.h b/toolbox/include/base/Parameter.h new file mode 100644 index 000000000..f3926ccbd --- /dev/null +++ b/toolbox/include/base/Parameter.h @@ -0,0 +1,96 @@ +#ifndef WBT_PARAMETER_H +#define WBT_PARAMETER_H + +#include +#include + +namespace wbt { + class ParameterMetadata; + template + class Parameter; + class Parameters; + enum ParameterType + { + // Scalar / Vector / Matrix + PARAM_INT, + PARAM_BOOL, + PARAM_DOUBLE, + PARAM_STRING, + // Cell + PARAM_CELL_INT, + PARAM_CELL_BOOL, + PARAM_CELL_DOUBLE, + PARAM_CELL_STRING, + // Struct + PARAM_STRUCT_INT, + PARAM_STRUCT_BOOL, + PARAM_STRUCT_DOUBLE, + PARAM_STRUCT_STRING, + PARAM_STRUCT_CELL_INT, + PARAM_STRUCT_CELL_BOOL, + PARAM_STRUCT_CELL_DOUBLE, + PARAM_STRUCT_CELL_STRING + }; + const int DYNAMIC_SIZE = -1; +} // namespace wbt + +class wbt::ParameterMetadata +{ +public: + 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); + inline bool operator!=(const ParameterMetadata& rhs) { return !(*this == rhs); } +}; + +template +class wbt::Parameter +{ +private: + typedef std::vector ParamVector; + + bool m_isScalar; + T m_valueScalar; + ParamVector m_valueVector; + wbt::ParameterMetadata m_metadata; + +public: + 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/include/base/Parameters.h b/toolbox/include/base/Parameters.h new file mode 100644 index 000000000..d689d52e7 --- /dev/null +++ b/toolbox/include/base/Parameters.h @@ -0,0 +1,445 @@ +#ifndef WBT_PARAMETERS_H +#define WBT_PARAMETERS_H + +#include + +#include +#include + +const int PARAM_INVALID_INDEX = -1; +const std::string PARAM_INVALID_NAME{}; + +namespace wbt { + class Parameters; +} + +class wbt::Parameters +{ +public: + typedef int ParamIndex; + typedef std::string ParamName; + +private: + typedef Parameter ParameterInt; + typedef Parameter ParameterBool; + typedef Parameter ParameterDouble; + typedef Parameter ParameterString; + + // Private typedefs + typedef std::vector ParamVectorInt; + typedef std::vector ParamVectorBool; + typedef std::vector ParamVectorDouble; + typedef std::vector ParamVectorString; + + // Maps for storing parameters and their metadata + std::unordered_map m_paramsInt; + std::unordered_map m_paramsBool; + std::unordered_map m_paramsDouble; + std::unordered_map m_paramsString; + + // Maps for handling the internal indexing + std::unordered_map m_nameToType; + // + std::unordered_map m_indexToName; + std::unordered_map m_nameToIndex; + + bool existIndex(const ParamIndex& index) const; + bool existName(const ParamName& name, const wbt::ParameterType& type) const; + + // Handle vector parameters + template + bool storeVectorParameter(const std::vector& param, + const wbt::ParameterMetadata& paramMetadata); + + // Convert std::vector between different data types + template + static void convertStdVector(const std::vector& input, std::vector& output); + +public: + Parameters() = default; + ~Parameters() = default; + + // Parameters(const Parameters& param) = default; + // Parameters(Parameters&& param); + // Parameters& operator=(const Parameters& param); + // Parameters& operator=(Parameters&& param); + + ParamName getParamName(const ParamIndex& index) const; + ParamIndex getParamIndex(const ParamName& name) const; + unsigned getNumberOfParameters() const; + + bool existName(const ParamName& name) const; + + // Scalar parameters + // TODO: template for numeric types + template + bool storeParameter(const T& param, const wbt::ParameterMetadata& paramMetadata); + // bool storeParameter(const int& param, const wbt::ParameterMetadata& paramMetadata); + // bool storeParameter(const bool& param, const wbt::ParameterMetadata& paramMetadata); + // bool storeParameter(const double& param, const wbt::ParameterMetadata& paramMetadata); + // bool storeParameter(const std::string& param, const wbt::ParameterMetadata& + // paramMetadata); + + // Vector parameters + // template bool storeParameter(const std::vector& param, const + // wbt::ParameterMetadata& paramMetadata); + // bool storeParameter(const ParamVectorInt& param, const wbt::ParameterMetadata& + // paramMetadata); bool storeParameter(const ParamVectorBool& param, const + // wbt::ParameterMetadata& paramMetadata); bool storeParameter(const ParamVectorDouble& + // param, const wbt::ParameterMetadata& paramMetadata); bool storeParameter(const + // ParamVectorString& param, const wbt::ParameterMetadata& paramMetadata); + + // Generic + template + bool storeParameter(const Parameter& parameter); + + // Scalar / Struct Fields + // TODO: template for numeric types + template + bool getParameter(const ParamName& name, T& param) const; + template + bool getParameter(const ParamName& name, std::vector& param) const; + // bool getParameter(const ParamName& name, int& param) const; + // bool getParameter(const ParamName& name, bool& param) const; + // bool getParameter(const ParamName& name, double& param) const; + // bool getParameter(const ParamName& name, std::string& param) const; + // Cells, Vectors, Matrices + // bool getParameter(const ParamName& name, ParamVectorInt& param) const; + // bool getParameter(const ParamName& name, ParamVectorBool& param) const; + // bool getParameter(const ParamName& name, ParamVectorDouble& param) const; + // bool getParameter(const ParamName& name, ParamVectorString& param) const; + // + // template + // const wbt::Parameter& getParameter(const ParamName& name); + // bool getParameter(const ParamName& name, Parameter& parameter); + + std::vector> getIntParameters() const; + std::vector> getBoolParameters() const; + std::vector> getDoubleParameters() const; + std::vector> getStringParameters() const; + + wbt::ParameterMetadata getParameterMetadata(const ParamName& name); + + static bool containConfigurationData(const wbt::Parameters& parameters); +}; + +// TEMPLATE storeParameter +// ======================= + +template <> +bool wbt::Parameters::getParameter(const wbt::Parameters::ParamName& name, + std::string& param) const; + +// Vector + +// template<> bool wbt::Parameters::getParameter(const wbt::Parameters::ParamName& name, +// std::vector& param) const; +// template<> bool wbt::Parameters::getParameter>(const +// wbt::Parameters::ParamName& name, +// std::vector& param) const; +// template<> bool wbt::Parameters::getParameter>(const +// wbt::Parameters::ParamName& name, +// std::vector& param) +// const; +template <> +bool wbt::Parameters::getParameter>(const wbt::Parameters::ParamName& name, + std::vector& param) const; + +template +bool wbt::Parameters::getParameter(const wbt::Parameters::ParamName& name, T& param) const +{ + if (!existName(name) || !existName(name, m_nameToType.at(name))) { + return false; + } + + switch (m_nameToType.at(name)) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: + if (!m_paramsInt.at(name).isScalar()) { + return false; + } + param = static_cast(m_paramsInt.at(name).getScalarParameter()); + break; + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: + if (!m_paramsBool.at(name).isScalar()) { + return false; + } + param = static_cast(m_paramsBool.at(name).getScalarParameter()); + break; + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: + if (!m_paramsDouble.at(name).isScalar()) { + return false; + } + param = static_cast(m_paramsDouble.at(name).getScalarParameter()); + break; + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: + if (!m_paramsString.at(name).isScalar()) { + return false; + } + // In order to have a general template, use std::stod and then cast to T + param = static_cast(std::stod(m_paramsString.at(name).getScalarParameter())); + break; + } + return true; +} + +// bool Parameters::getParameter(const ParamName& name, ParamVectorInt& param) const +template +bool wbt::Parameters::getParameter(const wbt::Parameters::ParamName& name, + std::vector& param) const +{ + if (!existName(name) || !existName(name, m_nameToType.at(name))) { + return false; + } + + param.clear(); + + switch (m_nameToType.at(name)) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: { + if (m_paramsInt.at(name).isScalar()) { + return false; + } + std::vector output; + convertStdVector(m_paramsInt.at(name).getVectorParameter(), param); + break; + } + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: { + if (m_paramsBool.at(name).isScalar()) { + return false; + } + std::vector output; + convertStdVector(m_paramsBool.at(name).getVectorParameter(), param); + // const auto vector = m_paramsBool.at(name).getVectorParameter(); + // param.assign(vector.begin(), vector.end()); + break; + } + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: { + if (m_paramsDouble.at(name).isScalar()) { + return false; + } + std::vector output; + convertStdVector(m_paramsDouble.at(name).getVectorParameter(), param); + // const auto vector = m_paramsDouble.at(name).getVectorParameter(); + // param.assign(vector.begin(), vector.end()); + break; + } + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: { + if (m_paramsString.at(name).isScalar()) { + return false; + } + std::vector output; + convertStdVector(m_paramsString.at(name).getVectorParameter(), param); + // const auto vector = m_paramsString.at(name).getVectorParameter(); + // param.reserve(vector.size()); + // for (auto element : vector) { + // param.push_back(static_cast(std::stod(element))); + // } + break; + } + } + return true; +} + +// TEMPLATE storeParameter +// ======================= + +// Specialize the template for non-numeric types +template <> +bool wbt::Parameters::storeParameter(const std::string& param, + const wbt::ParameterMetadata& paramMetadata); +template <> +bool wbt::Parameters::storeParameter>(const std::vector& param, + const wbt::ParameterMetadata& paramMetadata); +template <> +bool wbt::Parameters::storeParameter>( + const std::vector& param, const wbt::ParameterMetadata& paramMetadata); +template <> +bool wbt::Parameters::storeParameter>( + const std::vector& param, const wbt::ParameterMetadata& paramMetadata); +template <> +bool wbt::Parameters::storeParameter>( + const std::vector& param, const wbt::ParameterMetadata& paramMetadata); + +template +bool wbt::Parameters::storeParameter(const T& param, const wbt::ParameterMetadata& paramMetadata) +{ + if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { + return false; + } + + if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { + return false; + } + + switch (paramMetadata.type) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: + m_paramsInt.emplace(std::make_pair( + paramMetadata.name, ParameterInt(static_cast(param), paramMetadata))); + break; + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: + m_paramsBool.emplace(std::make_pair( + paramMetadata.name, ParameterBool(static_cast(param), paramMetadata))); + break; + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: + m_paramsDouble.emplace(std::make_pair( + paramMetadata.name, ParameterDouble(static_cast(param), paramMetadata))); + break; + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: + m_paramsString.emplace(std::make_pair( + paramMetadata.name, ParameterString(std::to_string(param), paramMetadata))); + break; + } + + m_nameToType[paramMetadata.name] = paramMetadata.type; + m_nameToIndex[paramMetadata.name] = paramMetadata.index; + m_indexToName[paramMetadata.index] = paramMetadata.name; + + return true; +} + +// Specialize the template for std::string +// template<> +// bool wbt::Parameters::storeParameter(const std::string& param, const wbt::ParameterMetadata& +// paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// m_paramsInt.emplace( +// std::make_pair(paramMetadata.name, +// ParameterInt(std::stoi(param), paramMetadata))); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// m_paramsBool.emplace( +// std::make_pair(paramMetadata.name, +// ParameterBool(static_cast(std::stoi(param)), paramMetadata))); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// m_paramsDouble.emplace( +// std::make_pair(paramMetadata.name, +// ParameterDouble(std::stod(param), paramMetadata))); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// m_paramsString.emplace( +// std::make_pair(paramMetadata.name, +// ParameterString(param, paramMetadata))); +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// template +// bool wbt::Parameters::getParameter(const ParamName& name, Parameter& parameter) +////const wbt::Parameter& wbt::Parameters::getParameter(const ParamName& name) +//// TODO: not tested +//// Usage: getParameter(paramName) +//{ +// if (!existName(name)) { +// return false; +// } + +// switch (getParameterMetadata(name).type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// parameter = m_paramsInt[name]; +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// parameter = m_paramsBool[name]; +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// parameter = m_paramsDouble[name]; +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// parameter = m_paramsString[name]; +// break; +// } + +// return true; +//} + +template +bool wbt::Parameters::storeParameter(const wbt::Parameter& parameter) +{ + if (existName(parameter.getMetadata().name)) { + return false; + } + + if (parameter.isScalar()) { + return storeParameter(parameter.getScalarParameter(), parameter.getMetadata()); + } + else { + return storeParameter(parameter.getVectorParameter(), parameter.getMetadata()); + } +} + +#endif // WBT_PARAMETERS_H diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h index ac9e5f538..cb6ed3637 100644 --- a/toolbox/include/base/RobotInterface.h +++ b/toolbox/include/base/RobotInterface.h @@ -2,7 +2,6 @@ #define WBT_ROBOTINTERFACE_H #include "Configuration.h" - #include #include #include diff --git a/toolbox/include/base/Signal.h b/toolbox/include/base/Signal.h index 7d163acee..dcd388b85 100644 --- a/toolbox/include/base/Signal.h +++ b/toolbox/include/base/Signal.h @@ -2,7 +2,6 @@ #define WBT_SIGNAL_H #include "BlockInformation.h" - #include #include @@ -116,9 +115,6 @@ T* wbt::Signal::getBuffer() const return nullptr; } break; - default: - return nullptr; - break; } // Return the correct pointer diff --git a/toolbox/include/base/SimulinkBlockInformation.h b/toolbox/include/base/SimulinkBlockInformation.h index 4b52dba51..6491c69f7 100644 --- a/toolbox/include/base/SimulinkBlockInformation.h +++ b/toolbox/include/base/SimulinkBlockInformation.h @@ -1,19 +1,22 @@ #ifndef WBT_SIMULINKBLOCKINFORMATION_H #define WBT_SIMULINKBLOCKINFORMATION_H -#include "AnyType.h" #include "BlockInformation.h" +#include "MxAnyType.h" #include "simstruc.h" +#include namespace wbt { - class SimulinkBlockInformation; class Signal; + class SimulinkBlockInformation; } // namespace wbt class wbt::SimulinkBlockInformation : public wbt::BlockInformation { private: SimStruct* simstruct; + std::string m_confBlockName; + std::vector paramsMetadata; PortDataType mapSimulinkToPortType(const DTypeId& typeId) const; DTypeId mapPortTypeToSimulink(const PortDataType& dataType) const; @@ -29,13 +32,38 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation // PARAMETERS METHODS // ================== + // TODO + bool addParameterMetadata(const wbt::ParameterMetadata& paramMD) override; + bool parseParameters(wbt::Parameters& parameters) override; - bool getStringParameterAtIndex(unsigned parameterIndex, - std::string& stringParameter) const override; - bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const override; - bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const override; - bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const override; - bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const override; + // TODO: move outside the interface + // Scalar parameters + bool getScalarParameterAtIndex(unsigned parameterIndex, double& value) const; + bool getBooleanParameterAtIndex(unsigned parameterIndex, bool& value) const; + bool getStringParameterAtIndex(unsigned parameterIndex, std::string& stringParameter) const; + // Struct parameters + bool getStringFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + std::string& stringParameter) const; + bool getScalarFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + double& value) const; + bool getBooleanFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + bool& value) const; + bool + getCellFieldAtIndex(unsigned parameterIndex, const std::string& fieldName, AnyCell& cell) const; + bool getVectorDoubleFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + std::vector& vector) const; + // + bool getCellAtIndex(unsigned parameterIndex, AnyCell& vector) const; + bool getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const; + bool getVectorAtIndex(unsigned parameterIndex, std::vector& vec) const; + + // static bool containConfigurationData(const wbt::Parameters& parameters); + // bool fillConfiguration(wbt::Configuration& configuration, const wbt::Parameters& + // parameters); // PORT INFORMATION SETTERS // ======================== @@ -58,6 +86,10 @@ class wbt::SimulinkBlockInformation : public wbt::BlockInformation int portWidth = DYNAMICALLY_SIZED) const override; wbt::Signal getOutputPortSignal(unsigned portNumber, int portWidth = DYNAMICALLY_SIZED) const override; + + // TODO + std::weak_ptr getRobotInterface() const override; + std::weak_ptr getKinDynComputations() const override; }; #endif /* end of include guard: WBT_SIMULINKBLOCKINFORMATION_H */ diff --git a/toolbox/include/base/ToolboxSingleton.h b/toolbox/include/base/ToolboxSingleton.h index 55841b425..043ad367c 100644 --- a/toolbox/include/base/ToolboxSingleton.h +++ b/toolbox/include/base/ToolboxSingleton.h @@ -2,7 +2,6 @@ #define WBT_ToolboxSingleton_H #include "Configuration.h" - #include #include #include @@ -11,6 +10,7 @@ namespace wbt { class ToolboxSingleton; class RobotInterface; + class Parameters; } // namespace wbt namespace iDynTree { @@ -57,7 +57,7 @@ class wbt::ToolboxSingleton * @param confKey The key describing the configuration (name of the Simulink block) * @return True if the configuration is valid */ - bool isKeyValid(const std::string& confKey); + bool isKeyValid(const std::string& confKey) const; /** * Returns the DoFs associated with the configuration labelled by confKey @@ -91,7 +91,7 @@ class wbt::ToolboxSingleton * @param confKey The key describing the configuration (name of the Simulink block) * @return A constant reference to the Configuration object */ - const Configuration& getConfiguration(const std::string& confKey); + const Configuration& getConfiguration(const std::string& confKey) const; /** * Returns a \c shared_ptr to the RobotInterface object containing the configuration @@ -100,7 +100,7 @@ class wbt::ToolboxSingleton * @param confKey The key describing the configuration (name of the Simulink block) * @return A \c shared_ptr to the RobotInterface of the requested configuration */ - const std::shared_ptr getRobotInterface(const std::string& confKey); + const std::shared_ptr getRobotInterface(const std::string& confKey) const; /** * Returns a \c shared_ptr to the KinDynComputations object used to perform calculation @@ -111,7 +111,7 @@ class wbt::ToolboxSingleton * configuration */ const std::shared_ptr - getKinDynComputations(const std::string& confKey); + getKinDynComputations(const std::string& confKey) const; // TOOLBOXSINGLETON CONFIGURATION // ============================== @@ -126,12 +126,25 @@ class wbt::ToolboxSingleton * subsequent compilation in Simulink, when the block's parameters have been * changed. * - * @param confKey The key describing the configuration (name of the Simulink block) * @param config The wbt::Configuration object parsed from Simulink's parameters * @return Returns \c true if configure is successful, \c false otherwise * @see ToolboxSingleton::isKeyValid */ - bool storeConfiguration(const std::string& confKey, const Configuration& config); + bool storeConfiguration(const Configuration& config); + + /*! Saves in the singleton a new configuration \c config. + * + * Same as ToolboxSingleton::storeConfiguration but taking a wbt::Parameters object + * as input. + * + * @param parameters A wbt::Parameters object containing all the parameters to fill + * a wbt::Configuration object + * @return Returns \c true if configure is successful, \c false otherwise + * @see ToolboxSingleton::storeConfiguration + * @see wbt::Configuration + * @see wbt::Parameters::containConfigurationData + */ + bool storeConfiguration(const wbt::Parameters& parameters); /** * Delete the wbt::RobotInterface referred by confKey. No-op if it doesn't exist. diff --git a/toolbox/include/base/WBBlock.h b/toolbox/include/base/WBBlock.h index 46ed3e6ab..b8cf96917 100644 --- a/toolbox/include/base/WBBlock.h +++ b/toolbox/include/base/WBBlock.h @@ -22,7 +22,8 @@ namespace wbt { namespace iDynTree { class MatrixDynSize; -} + class KinDynComputations; +} // namespace iDynTree /** * \struct iDynTreeRobotState WBBlock.h @@ -64,27 +65,28 @@ struct iDynTreeRobotState */ class wbt::WBBlock : public wbt::Block { -private: - static const unsigned ConfigurationParameterIndex; - static const unsigned ConfBlockNameParameterIndex; - protected: - std::string confKey; + // std::string confKey; iDynTreeRobotState robotState; - bool getWBToolboxParameters(Configuration& config, const BlockInformation* blockInfo); - const std::shared_ptr getRobotInterface(); - const Configuration& getConfiguration(); + // bool fillConfiguration(Configuration& config); + // const std::shared_ptr getRobotInterface() const; + // bool getConfiguration(BlockInformation* blockInfo, Configuration& configuration) const; + std::weak_ptr getKinDynComputations(BlockInformation* blockInfo); bool setRobotState(const wbt::Signal* basePose, const wbt::Signal* jointsPos, const wbt::Signal* baseVelocity, - const wbt::Signal* jointsVelocity); + const wbt::Signal* jointsVelocity, + iDynTree::KinDynComputations* kinDyn); public: + static const unsigned NumberOfParameters; + WBBlock() = default; ~WBBlock() override = default; unsigned numberOfParameters() override; + bool parseParameters(BlockInformation* blockInfo) override; bool configureSizeAndPorts(BlockInformation* blockInfo) override; - bool initialize(const BlockInformation* blockInfo) override; + bool initialize(BlockInformation* blockInfo) override; bool terminate(const BlockInformation* blockInfo) override; }; diff --git a/toolbox/include/base/toolbox.h b/toolbox/include/base/toolbox.h index 64301fac3..846b193e5 100644 --- a/toolbox/include/base/toolbox.h +++ b/toolbox/include/base/toolbox.h @@ -1,30 +1,26 @@ - -// Yarp-related blocks +#include "Log.h" +// General Yarp utilities +// #include "YarpRead.h" +// #include "YarpWrite.h" +// #include "ModelPartitioner.h" #include "YarpClock.h" -#include "YarpRead.h" -#include "YarpWrite.h" - -// Generic blocks -#include "CentroidalMomentum.h" -#include "DotJNu.h" -#include "ForwardKinematics.h" -#include "GetLimits.h" -#include "GetMeasurement.h" -#include "InverseDynamics.h" -#include "Jacobian.h" -#include "MassMatrix.h" -#include "ModelPartitioner.h" +// WBI-related stuff +// #include "MassMatrix.h" +// #include "ForwardKinematics.h" #include "RealTimeSynchronizer.h" -#include "SetLowLevelPID.h" #include "SetReferences.h" #include "SimulatorSynchronizer.h" - +// #include "Jacobian.h" +#include "GetMeasurement.h" +// #include "InverseDynamics.h" +#include "DotJNu.h" +// #include "GetLimits.h" +#include "CentroidalMomentum.h" +// #include "SetLowLevelPID.h" #ifdef WBT_USES_ICUB -// iCub-related blocks +// #include "MinimumJerkTrajectoryGenerator.h" #include "DiscreteFilter.h" -#include "MinimumJerkTrajectoryGenerator.h" #endif - #ifdef WBT_USES_IPOPT // #include "InverseKinematics.h" #endif diff --git a/toolbox/matlab/library/WBToolboxLibrary_repository.mdl b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl index f92f27f88..66ed125aa 100644 --- a/toolbox/matlab/library/WBToolboxLibrary_repository.mdl +++ b/toolbox/matlab/library/WBToolboxLibrary_repository.mdl @@ -86,9 +86,9 @@ Library { ModifiedByFormat "%" LastModifiedBy "dferigo" ModifiedDateFormat "%" - LastModifiedDate "Mon Feb 12 09:39:33 2018" - RTWModifiedTimeStamp 440329126 - ModelVersionFormat "1.%" + LastModifiedDate "Mon Feb 12 16:08:46 2018" + RTWModifiedTimeStamp 440352506 + ModelVersionFormat "1.%" SampleTimeColors off SampleTimeAnnotations off LibraryLinkDisplay "disabled" @@ -1589,7 +1589,7 @@ Library { ZOrder 103 BackgroundColor "yellow" FunctionName "WBToolbox" - Parameters "'DiscreteFilter',filterType,numCoeffs,denCoeffs,Fc,Ts,orderMedianFilter,y0,u0" + Parameters "'DiscreteFilter',filterStruct" SFunctionDeploymentMode off EnableBusSupport off SFcnIsStateOwnerBlock off @@ -1599,9 +1599,12 @@ Library { $ClassName "Simulink.Mask" Type "Discrete Filter" Description "This block wraps the Filter, FirstOrderLowPassFilter, and MedianFilter from iCub::ctrl." - Initialization "filterType = get_param(gcb, 'filterType');\nnumCoeffs = mat2str(numCoeffs);\ndenCoeffs = ma" - "t2str(denCoeffs);\n\ninitStatus = get_param(gcb, 'initStatus');\n\nif (strcmp(initStatus,'on'))\n y0 = mat2str" - "(y0);\n u0 = mat2str(u0);\nelse\n y0 = 'none';\n u0 = 'none';\nend\n\nclear initStatus" + Initialization "filterStruct = struct;\n\nfilterStruct.FilterType = filterType;\nfilterStruct.Fc = Fc;\nfilte" + "rStruct.Ts = Ts;\nfilterStruct.MedianOrder = orderMedianFilter;\nfilterStruct.NumCoeffs = numCoeffs;\nfilterStruc" + "t.DenCoeffs = denCoeffs;\nfilterStruct.InitStatus = initStatus;\nfilterStruct.y0 = y0;\nfilterStruct.u0 = u0;\n\n" + "%filterType = get_param(gcb, 'filterType');\n%numCoeffs = mat2str(numCoeffs);\n%denCoeffs = mat2str(denCoeffs);" + "\n\n%initStatus = get_param(gcb, 'initStatus');\n\n%if (strcmp(initStatus,'on'))\n% y0 = mat2str(y0);\n% u0" + " = mat2str(u0);\n%else\n% y0 = 'none';\n% u0 = 'none';\n%end\n\n%clear initStatus" Display "disp('Filter')" RunInitForIconRedraw "off" Array { @@ -1621,6 +1624,7 @@ Library { 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';" @@ -1628,8 +1632,8 @@ Library { ",'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;" + "lities',{'on','off','off','off','off','on','on',vis_init,'off'});\n howToCoeffs.Visible = 'off';\nend\n\n%clear f" + "ilterType initStatus p howToCoeffs vis_init;" } Object { $ObjectID 48 diff --git a/toolbox/matlab/library/exported/WBToolboxLibrary.slx b/toolbox/matlab/library/exported/WBToolboxLibrary.slx index 7a0400886..67ef27614 100644 Binary files a/toolbox/matlab/library/exported/WBToolboxLibrary.slx and b/toolbox/matlab/library/exported/WBToolboxLibrary.slx differ diff --git a/toolbox/matlab/setupForMatlabDebug.m.in b/toolbox/matlab/setupForMatlabDebug.m.in index 848d18981..418353259 100644 --- a/toolbox/matlab/setupForMatlabDebug.m.in +++ b/toolbox/matlab/setupForMatlabDebug.m.in @@ -2,15 +2,16 @@ fprintf('\nSetup for Debug\n'); installDir = '@CMAKE_INSTALL_PREFIX@'; mexDir = [installDir, filesep, 'mex']; -shareDir = '@WB-TOOLBOX_SHARE_DIR@'; +%shareDir = '@WB-TOOLBOX_SHARE_DIR@'; rmpath(mexDir) -rmpath(shareDir) +%rmpath(shareDir) +packageFolder = '@CMAKE_CURRENT_SOURCE_DIR@/matlab' +addpath(packageFolder) -buildDir = '@CMAKE_CURRENT_BINARY_DIR@/Debug'; - +buildDir = '@CMAKE_CURRENT_BINARY_DIR@'; addpath(buildDir) toolboxSourceDir = '@CMAKE_CURRENT_SOURCE_DIR@'; -cd(toolboxSourceDir); \ No newline at end of file +%cd(toolboxSourceDir); diff --git a/toolbox/src/CentroidalMomentum.cpp b/toolbox/src/CentroidalMomentum.cpp index 4f1d18100..a24608e9f 100644 --- a/toolbox/src/CentroidalMomentum.cpp +++ b/toolbox/src/CentroidalMomentum.cpp @@ -1,24 +1,23 @@ #include "CentroidalMomentum.h" + #include "BlockInformation.h" #include "Log.h" #include "RobotInterface.h" #include "Signal.h" - #include #include #include - #include using namespace wbt; const std::string CentroidalMomentum::ClassName = "CentroidalMomentum"; -const unsigned CentroidalMomentum::INPUT_IDX_BASE_POSE = 0; -const unsigned CentroidalMomentum::INPUT_IDX_JOINTCONF = 1; -const unsigned CentroidalMomentum::INPUT_IDX_BASE_VEL = 2; -const unsigned CentroidalMomentum::INPUT_IDX_JOINT_VEL = 3; -const unsigned CentroidalMomentum::OUTPUT_IDX_CENTRMOM = 0; +const unsigned INPUT_IDX_BASE_POSE = 0; +const unsigned INPUT_IDX_JOINTCONF = 1; +const unsigned INPUT_IDX_BASE_VEL = 2; +const unsigned INPUT_IDX_JOINT_VEL = 3; +const unsigned OUTPUT_IDX_CENTRMOM = 0; CentroidalMomentum::CentroidalMomentum() {} @@ -44,7 +43,13 @@ bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) return false; } - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // Get the DoFs + if (!blockInfo->getRobotInterface().lock()) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); + return false; + } + const unsigned dofs = + blockInfo->getRobotInterface().lock()->getConfiguration().getNumberOfDoFs(); // Size and type bool success = true; @@ -82,7 +87,7 @@ bool CentroidalMomentum::configureSizeAndPorts(BlockInformation* blockInfo) return success; } -bool CentroidalMomentum::initialize(const BlockInformation* blockInfo) +bool CentroidalMomentum::initialize(BlockInformation* blockInfo) { if (!WBBlock::initialize(blockInfo)) return false; @@ -102,9 +107,9 @@ bool CentroidalMomentum::terminate(const BlockInformation* blockInfo) bool CentroidalMomentum::output(const BlockInformation* blockInfo) { - const auto& model = getRobotInterface()->getKinDynComputations(); - - if (!model) { + // Get the KinDynComputations object + auto kinDyn = getKinDynComputations(const_cast(blockInfo)).lock(); + if (!kinDyn) { Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); return false; } @@ -117,8 +122,8 @@ bool CentroidalMomentum::output(const BlockInformation* blockInfo) Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - bool ok = - setRobotState(&basePoseSig, &jointsPosSig, &baseVelocitySignal, &jointsVelocitySignal); + bool ok = setRobotState( + &basePoseSig, &jointsPosSig, &baseVelocitySignal, &jointsVelocitySignal, kinDyn.get()); if (!ok) { Log::getSingleton().error("Failed to set the robot state."); @@ -129,11 +134,17 @@ bool CentroidalMomentum::output(const BlockInformation* blockInfo) // ====== // Calculate the centroidal momentum - *m_centroidalMomentum = model->getCentroidalTotalMomentum(); + *m_centroidalMomentum = kinDyn->getCentroidalTotalMomentum(); - // Forward the output to Simulink + // Get the output signal Signal output = blockInfo->getOutputPortSignal(OUTPUT_IDX_CENTRMOM); - output.setBuffer(toEigen(*m_centroidalMomentum).data(), - blockInfo->getOutputPortWidth(OUTPUT_IDX_CENTRMOM)); + if (!output.getBuffer()) { + Log::getSingleton().error("Failed to get output signal."); + return false; + } + + // Fill the output buffer + output.setBuffer(toEigen(*m_centroidalMomentum).data(), output.getWidth()); + return true; } diff --git a/toolbox/src/DiscreteFilter.cpp b/toolbox/src/DiscreteFilter.cpp index 0f62b0bf3..9a5bfd6c6 100644 --- a/toolbox/src/DiscreteFilter.cpp +++ b/toolbox/src/DiscreteFilter.cpp @@ -1,15 +1,14 @@ #include "DiscreteFilter.h" #include "BlockInformation.h" #include "Log.h" +#include "Parameters.h" #include "Signal.h" - -#include -#include - #include #include +#include #include #include +#include using namespace wbt; using namespace iCub::ctrl; @@ -18,27 +17,67 @@ using namespace yarp::sig; const std::string DiscreteFilter::ClassName = "DiscreteFilter"; // Parameters -const unsigned DiscreteFilter::PARAM_IDX_FLT_TYPE = 1; // ::Filter type -const unsigned DiscreteFilter::PARAM_IDX_NUMCOEFF = 2; // ::Filter numerator coefficients -const unsigned DiscreteFilter::PARAM_IDX_DENCOEFF = 3; // ::Filter denominator coefficients -const unsigned DiscreteFilter::PARAM_IDX_1LOWP_FC = 4; // ::FirstOrderLowPassFilter cut frequency -const unsigned DiscreteFilter::PARAM_IDX_1LOWP_TS = 5; // ::FirstOrderLowPassFilter sampling time -const unsigned DiscreteFilter::PARAM_IDX_MD_ORDER = 6; // ::MedianFilter order -const unsigned DiscreteFilter::PARAM_IDX_INIT_Y0 = 7; // Output initialization -const unsigned DiscreteFilter::PARAM_IDX_INIT_U0 = 8; // ::Filter input initialization - -// Inputs +const unsigned PARAM_IDX_BIAS = Block::NumberOfParameters - 1; +const unsigned PARAM_IDX_STRUCT = PARAM_IDX_BIAS + 1; // Struct containing filter parameters + +// Inputs / Outputs const unsigned DiscreteFilter::INPUT_IDX_SIGNAL = 0; -// Outputs const unsigned DiscreteFilter::OUTPUT_IDX_SIGNAL = 0; -// Other defines + +// Other constants const int DiscreteFilter::SIGNAL_DYNAMIC_SIZE = -1; DiscreteFilter::DiscreteFilter() {} unsigned DiscreteFilter::numberOfParameters() { - return 8; + return Block::numberOfParameters() + 1; +} + +bool DiscreteFilter::parseParameters(BlockInformation* blockInfo) +{ + if (!Block::parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse Block parameters."); + return false; + } + + // Gather the parameters + // ===================== + + // Set scalar parameters metadata + ParameterMetadata paramMD_1lowp_fc(PARAM_STRUCT_DOUBLE, PARAM_IDX_STRUCT, 1, 1, "Fc"); + ParameterMetadata paramMD_1lowp_ts(PARAM_STRUCT_DOUBLE, PARAM_IDX_STRUCT, 1, 1, "Ts"); + ParameterMetadata paramMD_md_order(PARAM_STRUCT_INT, PARAM_IDX_STRUCT, 1, 1, "MedianOrder"); + + // Set string parameters metadata + ParameterMetadata paramMD_flt_type(PARAM_STRUCT_STRING, PARAM_IDX_STRUCT, 1, 1, "FilterType"); + ParameterMetadata paramMD_numcoeff( + PARAM_STRUCT_DOUBLE, PARAM_IDX_STRUCT, 1, DYNAMIC_SIZE, "NumCoeffs"); + ParameterMetadata paramMD_dencoeff( + PARAM_STRUCT_DOUBLE, PARAM_IDX_STRUCT, 1, DYNAMIC_SIZE, "DenCoeffs"); + ParameterMetadata paramMD_init_status(PARAM_STRUCT_BOOL, PARAM_IDX_STRUCT, 1, 1, "InitStatus"); + ParameterMetadata paramMD_init_y0(PARAM_STRUCT_DOUBLE, PARAM_IDX_STRUCT, 1, DYNAMIC_SIZE, "y0"); + ParameterMetadata paramMD_init_u0(PARAM_STRUCT_DOUBLE, PARAM_IDX_STRUCT, 1, DYNAMIC_SIZE, "u0"); + + // Store the parameters metadata in the BlockInformation object + bool ok = true; + // ok = ok && blockInfo->addParameterMetadata(paramMD_className); // TODO + ok = ok && blockInfo->addParameterMetadata(paramMD_1lowp_fc); + ok = ok && blockInfo->addParameterMetadata(paramMD_1lowp_ts); + ok = ok && blockInfo->addParameterMetadata(paramMD_md_order); + ok = ok && blockInfo->addParameterMetadata(paramMD_flt_type); + ok = ok && blockInfo->addParameterMetadata(paramMD_numcoeff); + ok = ok && blockInfo->addParameterMetadata(paramMD_dencoeff); + ok = ok && blockInfo->addParameterMetadata(paramMD_init_status); + ok = ok && blockInfo->addParameterMetadata(paramMD_init_y0); + ok = ok && blockInfo->addParameterMetadata(paramMD_init_u0); + + if (!ok) { + Log::getSingleton().error("Failed to store parameters metadata."); + return false; + } + + return blockInfo->parseParameters(parameters); } bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo) @@ -79,93 +118,163 @@ bool DiscreteFilter::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool DiscreteFilter::initialize(const BlockInformation* blockInfo) +bool DiscreteFilter::initialize(BlockInformation* blockInfo) { - // Handle the parameters - // ===================== + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); + return false; + } + + // Get the parameters - // Variables for the filter parameters std::string filter_type; - std::string num_coeff_str; - std::string den_coeff_str; - std::string y0_str; - std::string u0_str; + // std::string num_coeff_str; + // std::string den_coeff_str; + std::vector num_coeff; + std::vector den_coeff; + // std::string y0_str; + // std::string u0_str; + std::vector y0; + std::vector u0; double firstOrderLowPassFilter_fc; double firstOrderLowPassFilter_ts; - double medianFilter_order; + int medianFilter_order; + bool initStatus; - // Get the scalar parameters bool ok = true; - ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_FC, firstOrderLowPassFilter_fc); - ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_1LOWP_TS, firstOrderLowPassFilter_ts); - ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_IDX_MD_ORDER, medianFilter_order); - - // Get the string parameter - ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_FLT_TYPE, filter_type); - ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_NUMCOEFF, num_coeff_str); - ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_DENCOEFF, den_coeff_str); - ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_Y0, y0_str); - ok = ok && blockInfo->getStringParameterAtIndex(PARAM_IDX_INIT_U0, u0_str); + ok = ok && parameters.getParameter("FilterType", filter_type); + ok = ok && parameters.getParameter("NumCoeffs", num_coeff); + ok = ok && parameters.getParameter("DenCoeffs", den_coeff); + ok = ok && parameters.getParameter("y0", y0); + ok = ok && parameters.getParameter("u0", u0); + ok = ok && parameters.getParameter("InitStatus", initStatus); + ok = ok && parameters.getParameter("Fc", firstOrderLowPassFilter_fc); + ok = ok && parameters.getParameter("Ts", firstOrderLowPassFilter_ts); + ok = ok && parameters.getParameter("MedianOrder", medianFilter_order); if (!ok) { - Log::getSingleton().error("Failed to parse parameters."); + Log::getSingleton().error("Failed to get parameters after their parsing."); return false; } - // Convert the strings from the Matlab syntax ("[1.0 2 3.33]") to yarp::sig::Vector - yarp::sig::Vector num(0); - yarp::sig::Vector den(0); - stringToYarpVector(num_coeff_str, num); - stringToYarpVector(den_coeff_str, den); + // fprintf(stderr, "FilterType: %s\n", filter_type.c_str()); + // fprintf(stderr, "InitStatus: %d\n", static_cast(initStatus)); + // fprintf(stderr, "MedianFilderOrd: %d\n", static_cast(medianFilter_order)); + // fprintf(stderr, "Fc: %f\n", firstOrderLowPassFilter_fc); + // fprintf(stderr, "Ts: %f\n", firstOrderLowPassFilter_ts); + + // fprintf(stderr, "u0\n"); + // for (auto el : u0) { + // fprintf(stderr, "%f ", el); + // } + // fprintf(stderr, "\n"); + + // fprintf(stderr, "y0\n"); + // for (auto el : y0) { + // fprintf(stderr, "%f ", el); + // } + // fprintf(stderr, "\n"); + + // fprintf(stderr, "numCoeff\n"); + // for (auto el : num_coeff) { + // fprintf(stderr, "%f ", el); + // } + // fprintf(stderr, "\n"); + + // fprintf(stderr, "denCoeff\n"); + // for (auto el : den_coeff) { + // fprintf(stderr, "%f ", el); + // } + // fprintf(stderr, "\n"); + + // return false; + + // Configure the class + // =================== + + // Check if numerator and denominator are not empty + if (num_coeff.empty() || den_coeff.empty()) { + wbt::Log::getSingleton().error("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)) { + wbt::Log::getSingleton().error("Passed numerator or denominator not valid."); + return false; + } - // y0 and u0 are none if they are not defined - unsigned y0Width, u0Width; + // Convert the std::vector to yarp::sig::Vector + yarp::sig::Vector num(num_coeff.size(), num_coeff.data()); + yarp::sig::Vector den(den_coeff.size(), den_coeff.data()); - if (y0_str != "none") { - y0 = std::unique_ptr(new Vector(0)); - stringToYarpVector(y0_str, *y0); - y0Width = y0->length(); - } + // Get the width of the input vector + const unsigned inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); - if (u0_str != "none") { - u0 = std::unique_ptr(new Vector(0)); - stringToYarpVector(u0_str, *u0); - u0Width = u0->length(); + if (initStatus) { + // y0 and output signal dimensions should match + unsigned outputSignalWidth = blockInfo->getOutputPortWidth(OUTPUT_IDX_SIGNAL); + if (y0.size() != outputSignalWidth) { + Log::getSingleton().error("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() != inputSignalWidth)) { + Log::getSingleton().error("(Generic) u0 and input signal sizes don't match."); + return false; + } + // Allocate the initial conditions + m_y0 = std::unique_ptr(new Vector(y0.size(), y0.data())); + m_u0 = std::unique_ptr(new Vector(u0.size(), u0.data())); + } + else { + // Initialize zero initial conditions + m_y0 = std::unique_ptr(new Vector(y0.size(), 0.0)); + m_u0 = std::unique_ptr(new Vector(u0.size(), 0.0)); } + // // y0 and u0 are none if they are not defined + // unsigned y0Width{0}, u0Width{0}; + + // if (y0_str != "none") { + // y0 = std::unique_ptr(new Vector(0)); + // stringToYarpVector(y0_str, *y0); + // y0Width = y0->length(); + // } + + // if (u0_str != "none") { + // u0 = std::unique_ptr(new Vector(0)); + // stringToYarpVector(u0_str, *u0); + // u0Width = u0->length(); + // } + // Create the filter object // ======================== // Generic // ------- if (filter_type == "Generic") { - if (num.length() == 0 || den.length() == 0) { - Log::getSingleton().error("(Generic) Wrong coefficients size."); - return false; - } - filter = std::unique_ptr(new Filter(num, den)); + m_filter = std::unique_ptr(new Filter(num, den)); } // FirstOrderLowPassFilter // ----------------------- else if (filter_type == "FirstOrderLowPassFilter") { - if (firstOrderLowPassFilter_fc == 0 || firstOrderLowPassFilter_ts == 0) { + if (firstOrderLowPassFilter_fc == 0.0 || firstOrderLowPassFilter_ts == 0.0) { Log::getSingleton().error("(FirstOrderLowPassFilter) You need to " "specify Fc and Ts."); return false; } - filter = std::unique_ptr( + m_filter = std::unique_ptr( new FirstOrderLowPassFilter(firstOrderLowPassFilter_fc, firstOrderLowPassFilter_ts)); } // MedianFilter // ------------ else if (filter_type == "MedianFilter") { if (static_cast(medianFilter_order) == 0) { - Log::getSingleton().error("(MedianFilter) You need to specify the " - "filter order."); + Log::getSingleton().error("(MedianFilter) You need to specify the filter order."); return false; } - filter = - std::unique_ptr(new MedianFilter(static_cast(medianFilter_order))); + m_filter = std::unique_ptr(new MedianFilter(static_cast(medianFilter_order))); } else { Log::getSingleton().error("Filter type not recognized."); @@ -176,23 +285,23 @@ bool DiscreteFilter::initialize(const BlockInformation* blockInfo) // ========================= // Get the width of the input vector - inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); + // inputSignalWidth = blockInfo->getInputPortWidth(INPUT_IDX_SIGNAL); - // Check the initial conditions are properly sized - unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); + // // Check the initial conditions are properly sized + // unsigned outputSignalWidth = blockInfo->getOutputPortWidth(OUTPUT_IDX_SIGNAL); - if ((y0 != nullptr) && (y0Width != outputSignalWidth)) { - Log::getSingleton().error("y0 and output signal sizes don't match."); - return false; - } + // if (y0 && (y0Width != outputSignalWidth)) { + // Log::getSingleton().error("y0 and output signal sizes don't match."); + // return false; + // } - if ((u0 != nullptr) && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { - Log::getSingleton().error("(Generic) u0 and input signal sizes don't match."); - return false; - } + // if (u0 && (filter_type == "Generic") && (u0Width != inputSignalWidth)) { + // Log::getSingleton().error("(Generic) u0 and input signal sizes don't match."); + // return false; + // } // Allocate the input signal - inputSignalVector = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); + m_inputSignalVector = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); return true; } @@ -209,24 +318,43 @@ bool DiscreteFilter::initializeInitialConditions(const BlockInformation* blockIn // If the initial conditions for the output are not set, allocate a properly // sized vector - if (y0 == nullptr) { - unsigned outputSignalWidth = blockInfo->getInputPortWidth(OUTPUT_IDX_SIGNAL); - y0 = std::unique_ptr(new Vector(outputSignalWidth, 0.0)); - } - if (u0 == nullptr) { - u0 = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); + // bool initStatus {false}; + // parameters.getParameter("InitStatus", initStatus); + + // Check that initial conditions are initialized + if (!(m_y0 && m_u0)) { + Log::getSingleton().error("Initial conditions not initialized."); + return false; } + // if (!y0) { + // unsigned outputSignalWidth = blockInfo->getOutputPortWidth(OUTPUT_IDX_SIGNAL); + // y0 = std::unique_ptr(new Vector(outputSignalWidth, 0.0)); + // } + // if (!u0) { + // u0 = std::unique_ptr(new Vector(inputSignalWidth, 0.0)); + // } + // Initialize the filter. This is required because if the signal is not 1D, - // the default filter constructor initialize a wrongly sized y0 + // the default filter constructor initialize a wrongly sized y0. // Moreover, the Filter class has a different constructor that handles the - // zero-gain case - Filter* filter_c = dynamic_cast(filter.get()); - if (filter_c != nullptr) { - filter_c->init(*y0, *u0); + // zero-gain case. + IFilter* baseFilter = m_filter.get(); + Filter* filter_c = nullptr; + filter_c = dynamic_cast(baseFilter); + if (filter_c) { + // The filter is a Filter object + filter_c->init(*m_y0, *m_u0); } else { - filter->init(*y0); + // The filter is not a Filter object + if (m_filter) { + m_filter->init(*m_y0); + } + else { + wbt::Log::getSingleton().error("Failed to get the IFilter object."); + return false; + } } return true; @@ -234,21 +362,20 @@ bool DiscreteFilter::initializeInitialConditions(const BlockInformation* blockIn bool DiscreteFilter::output(const BlockInformation* blockInfo) { - if (filter == nullptr) { + if (!m_filter) return false; - } // Get the input and output signals Signal inputSignal = blockInfo->getInputPortSignal(INPUT_IDX_SIGNAL); Signal outputSignal = blockInfo->getOutputPortSignal(OUTPUT_IDX_SIGNAL); // Copy the Signal to the data structure that the filter wants - for (unsigned i = 0; i < inputSignalWidth; ++i) { - (*inputSignalVector)[i] = inputSignal.get(i); + for (unsigned i = 0; i < inputSignal.getWidth(); ++i) { + (*m_inputSignalVector)[i] = inputSignal.get(i); } // Filter the current component of the input signal - const Vector& outputVector = filter->filt(*inputSignalVector); + const Vector& outputVector = m_filter->filt(*m_inputSignalVector); // Forward the filtered signals to the output port outputSignal.setBuffer(outputVector.data(), outputVector.length()); @@ -256,27 +383,26 @@ bool DiscreteFilter::output(const BlockInformation* blockInfo) return true; } -void DiscreteFilter::stringToYarpVector(const std::string str, Vector& v) -{ - std::string s = str; - - // Lambda expression used to remove "[]," carachters - // TODO: what about removing everything but digits and "."? - auto lambda_remove_chars = [](const char& c) { - if ((c == '[') || (c == ']') || c == ',') - return true; - else - return false; - }; - - // Apply the lambda expression the input parameters - s.erase(remove_if(s.begin(), s.end(), lambda_remove_chars), s.end()); - - // Convert the cleaned string to a yarp vector of floats - std::istringstream sstrm(s); - float f; - - while (sstrm >> f) { - v.push_back(f); - } -} +// void DiscreteFilter::stringToYarpVector(const std::string str, Vector& v) +//{ +// std::string s = str; + +// // Lambda expression used to remove "[]," carachters +// // TODO: what about removing everything but digits and "."? +// auto lambda_remove_chars = [](const char& c) { +// if ((c == '[') || (c == ']') || c == ',') +// return true; +// else +// return false; +// }; + +// // Apply the lambda expression the input parameters +// s.erase(remove_if(s.begin(), s.end(), lambda_remove_chars), s.end()); + +// // Convert the cleaned string to a yarp vector of floats +// std::istringstream sstrm(s); +// float f; + +// while (sstrm >> f) +// v.push_back(f); +//} diff --git a/toolbox/src/DotJNu.cpp b/toolbox/src/DotJNu.cpp index 9412f17e8..68c071ec8 100644 --- a/toolbox/src/DotJNu.cpp +++ b/toolbox/src/DotJNu.cpp @@ -1,23 +1,25 @@ #include "DotJNu.h" + #include "BlockInformation.h" #include "Log.h" #include "RobotInterface.h" #include "Signal.h" - #include #include - #include using namespace wbt; const std::string DotJNu::ClassName = "DotJNu"; -const unsigned DotJNu::INPUT_IDX_BASE_POSE = 0; -const unsigned DotJNu::INPUT_IDX_JOINTCONF = 1; -const unsigned DotJNu::INPUT_IDX_BASE_VEL = 2; -const unsigned DotJNu::INPUT_IDX_JOINT_VEL = 3; -const unsigned DotJNu::OUTPUT_IDX_DOTJ_NU = 0; +const unsigned INPUT_IDX_BASE_POSE = 0; +const unsigned INPUT_IDX_JOINTCONF = 1; +const unsigned INPUT_IDX_BASE_VEL = 2; +const unsigned INPUT_IDX_JOINT_VEL = 3; +const unsigned OUTPUT_IDX_DOTJ_NU = 0; + +const unsigned PARAM_IDX_BIAS = WBBlock::NumberOfParameters - 1; +const unsigned PARAM_IDX_FRAME = PARAM_IDX_BIAS + 1; DotJNu::DotJNu() : m_frameIsCoM(false) @@ -29,6 +31,18 @@ unsigned DotJNu::numberOfParameters() return WBBlock::numberOfParameters() + 1; } +bool DotJNu::parseParameters(BlockInformation* blockInfo) +{ + ParameterMetadata frameMetadata(PARAM_STRING, PARAM_IDX_FRAME, 1, 1, "frame"); + + if (!blockInfo->addParameterMetadata(frameMetadata)) { + Log::getSingleton().error("Failed to store parameters metadata."); + return false; + } + + return blockInfo->parseParameters(parameters); +} + bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) { // Memory allocation / Saving data not allowed here @@ -51,7 +65,13 @@ bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) return false; } - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // Get the DoFs + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); + return false; + } + const auto dofs = robotInterface->getConfiguration().getNumberOfDoFs(); // Size and type bool success = true; @@ -89,7 +109,7 @@ bool DotJNu::configureSizeAndPorts(BlockInformation* blockInfo) return success; } -bool DotJNu::initialize(const BlockInformation* blockInfo) +bool DotJNu::initialize(BlockInformation* blockInfo) { if (!WBBlock::initialize(blockInfo)) { return false; @@ -98,10 +118,13 @@ bool DotJNu::initialize(const BlockInformation* blockInfo) // INPUT PARAMETERS // ================ - std::string frame; - int parentParameters = WBBlock::numberOfParameters(); + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); + return false; + } - if (!blockInfo->getStringParameterAtIndex(parentParameters + 1, frame)) { + std::string frame; + if (!parameters.getParameter("frame", frame)) { Log::getSingleton().error("Cannot retrieve string from frame parameter."); return false; } @@ -109,14 +132,14 @@ bool DotJNu::initialize(const BlockInformation* blockInfo) // Check if the frame is valid // --------------------------- - const auto& model = getRobotInterface()->getKinDynComputations(); - if (!model) { + auto kinDyn = getKinDynComputations(blockInfo).lock(); + if (!kinDyn) { Log::getSingleton().error("Cannot retrieve handle to KinDynComputations."); return false; } if (frame != "com") { - m_frameIndex = model->getFrameIndex(frame); + m_frameIndex = kinDyn->getFrameIndex(frame); if (m_frameIndex == iDynTree::FRAME_INVALID_INDEX) { Log::getSingleton().error("Cannot find " + frame + " in the frame list."); return false; @@ -142,9 +165,14 @@ bool DotJNu::terminate(const BlockInformation* blockInfo) bool DotJNu::output(const BlockInformation* blockInfo) { - const auto& model = getRobotInterface()->getKinDynComputations(); + // Get the KinDynComputations object + auto kinDyn = blockInfo->getKinDynComputations().lock(); + if (!kinDyn) { + Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + return false; + } - if (!model) { + if (!kinDyn) { Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); return false; } @@ -157,8 +185,8 @@ bool DotJNu::output(const BlockInformation* blockInfo) Signal baseVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_BASE_VEL); Signal jointsVelocitySignal = blockInfo->getInputPortSignal(INPUT_IDX_JOINT_VEL); - bool ok = - setRobotState(&basePoseSig, &jointsPosSig, &baseVelocitySignal, &jointsVelocitySignal); + bool ok = setRobotState( + &basePoseSig, &jointsPosSig, &baseVelocitySignal, &jointsVelocitySignal, kinDyn.get()); if (!ok) { Log::getSingleton().error("Failed to set the robot state."); @@ -169,10 +197,10 @@ bool DotJNu::output(const BlockInformation* blockInfo) // ====== if (!m_frameIsCoM) { - *m_dotJNu = model->getFrameBiasAcc(m_frameIndex); + *m_dotJNu = kinDyn->getFrameBiasAcc(m_frameIndex); } else { - iDynTree::Vector3 comBiasAcc = model->getCenterOfMassBiasAcc(); + iDynTree::Vector3 comBiasAcc = kinDyn->getCenterOfMassBiasAcc(); toEigen(*m_dotJNu).segment<3>(0) = iDynTree::toEigen(comBiasAcc); toEigen(*m_dotJNu).segment<3>(3).setZero(); } diff --git a/toolbox/src/GetLimits.cpp b/toolbox/src/GetLimits.cpp index 0dc31de2d..a80fb6cf2 100644 --- a/toolbox/src/GetLimits.cpp +++ b/toolbox/src/GetLimits.cpp @@ -1,15 +1,14 @@ #include "GetLimits.h" + #include "BlockInformation.h" #include "Log.h" #include "RobotInterface.h" #include "Signal.h" - #include #include -#include - #include #include +#include #define _USE_MATH_DEFINES #include @@ -28,12 +27,30 @@ unsigned GetLimits::numberOfParameters() return WBBlock::numberOfParameters() + 1; } -bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) +bool GetLimits::parseParameters(BlockInformation* blockInfo) { - if (!WBBlock::configureSizeAndPorts(blockInfo)) { + if (!WBBlock::parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse WBBlock parameters."); return false; } + ParameterMetadata paramMD_filterType(PARAM_DOUBLE, WBBlock::numberOfParameters() + 1, 1, 1); + + bool ok = blockInfo->addParameterMetadata(paramMD_filterType); + + if (!ok) { + Log::getSingleton().error("Failed to store parameters metadata."); + return false; + } + + return blockInfo->parseParameters(parameters); +} + +bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) + return false; + // INPUTS // ====== // @@ -56,6 +73,7 @@ bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) return false; } + // TODO: this parses the parameters again const unsigned dofs = getConfiguration().getNumberOfDoFs(); bool success = true; @@ -73,21 +91,33 @@ bool GetLimits::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool GetLimits::initialize(const BlockInformation* blockInfo) +bool GetLimits::initialize(BlockInformation* blockInfo) { using namespace yarp::os; + if (!WBBlock::initialize(blockInfo)) + return false; - if (!WBBlock::initialize(blockInfo)) { + // TODO: what is the best order to parse parameters? + // We have WBBlock and block's one, in both initializeInitialConditions and initialize + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); return false; } // Read the control type std::string limitType; - if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType)) { - Log::getSingleton().error("Could not read estimate type parameter."); + if (!parameters.getScalarParameter(WBBlock::numberOfParameters() + 1, limitType)) { + Log::getSingleton().error("Failed to get parameters after their parsing."); return false; } + // // Read the control type + // std::string limitType; + // if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType)) { + // Log::getSingleton().error("Could not read estimate type parameter."); + // return false; + // } + // Initialize the structure that stores the limits const unsigned dofs = getConfiguration().getNumberOfDoFs(); m_limits.reset(new Limit(dofs)); @@ -208,12 +238,19 @@ bool GetLimits::terminate(const BlockInformation* blockInfo) // Read the control type std::string limitType; - ok = ok && blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, limitType); - if (!ok) { - Log::getSingleton().error("Could not read estimate type parameter."); + if (!parameters.getScalarParameter(WBBlock::numberOfParameters() + 1, limitType)) { + Log::getSingleton().error("Failed to get parameters after their parsing."); // Don't return false here. WBBlock::terminate must be called in any case } + // // Read the control type + // std::string limitType; + // ok = ok && blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, + // limitType); if (!ok) { + // Log::getSingleton().error("Could not read estimate type parameter."); + // // Don't return false here. WBBlock::terminate must be called in any case + // } + // Release the RemoteControlBoardRemapper if (limitType == "ControlBoardPosition" || limitType == "ControlBoardVelocity") { ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); @@ -228,9 +265,8 @@ bool GetLimits::terminate(const BlockInformation* blockInfo) bool GetLimits::output(const BlockInformation* blockInfo) { - if (!m_limits) { + if (!m_limits) return false; - } Signal minPort = blockInfo->getOutputPortSignal(0); Signal maxPort = blockInfo->getOutputPortSignal(1); diff --git a/toolbox/src/GetMeasurement.cpp b/toolbox/src/GetMeasurement.cpp index 2c116656a..fdb8e7350 100644 --- a/toolbox/src/GetMeasurement.cpp +++ b/toolbox/src/GetMeasurement.cpp @@ -1,9 +1,9 @@ #include "GetMeasurement.h" + #include "BlockInformation.h" #include "Log.h" #include "RobotInterface.h" #include "Signal.h" - #include #include @@ -14,6 +14,9 @@ using namespace wbt; const std::string GetMeasurement::ClassName = "GetMeasurement"; +const unsigned PARAM_IDX_BIAS = WBBlock::NumberOfParameters - 1; +const unsigned PARAM_IDX_MEAS_TYPE = PARAM_IDX_BIAS + 1; + void GetMeasurement::deg2rad(std::vector& v) { const double Deg2Rad = M_PI / 180.0; @@ -27,12 +30,31 @@ unsigned GetMeasurement::numberOfParameters() return WBBlock::numberOfParameters() + 1; } -bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) +bool GetMeasurement::parseParameters(BlockInformation* blockInfo) { - if (!WBBlock::configureSizeAndPorts(blockInfo)) { + // Gather the parameters + // ===================== + + // Set parameters metadata + ParameterMetadata paramMD_measType(PARAM_STRING, PARAM_IDX_MEAS_TYPE, 1, 1, "MeasuredType"); + + // Store the parameters metadata in the BlockInformation object + bool ok = true; + ok = ok && blockInfo->addParameterMetadata(paramMD_measType); + + if (!ok) { + Log::getSingleton().error("Failed to store parameters metadata."); return false; } + return blockInfo->parseParameters(parameters); +} + +bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) +{ + if (!WBBlock::configureSizeAndPorts(blockInfo)) + return false; + // INPUTS // ====== // @@ -55,7 +77,13 @@ bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) return false; } - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // Get the DoFs + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); + return false; + } + const auto dofs = robotInterface->getConfiguration().getNumberOfDoFs(); bool success = blockInfo->setOutputPortVectorSize(0, dofs); blockInfo->setOutputPortType(0, PortDataTypeDouble); @@ -67,42 +95,54 @@ bool GetMeasurement::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool GetMeasurement::initialize(const BlockInformation* blockInfo) +bool GetMeasurement::initialize(BlockInformation* blockInfo) { - if (!WBBlock::initialize(blockInfo)) { + if (!WBBlock::initialize(blockInfo)) + return false; + + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); return false; } - // Reading the control type - std::string informationType; - if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, informationType)) { - Log::getSingleton().error("Could not read estimate type parameter."); + // Read the measured type + std::string measuredType; + if (!parameters.getParameter("MeasuredType", measuredType)) { + Log::getSingleton().error("Could not read measured type parameter."); return false; } - if (informationType == "Joints Position") { + // Set the measured type + if (measuredType == "Joints Position") { m_measuredType = wbt::MEASUREMENT_JOINT_POS; } - else if (informationType == "Joints Velocity") { + else if (measuredType == "Joints Velocity") { m_measuredType = wbt::MEASUREMENT_JOINT_VEL; } - else if (informationType == "Joints Acceleration") { + else if (measuredType == "Joints Acceleration") { m_measuredType = wbt::MEASUREMENT_JOINT_ACC; } - else if (informationType == "Joints Torque") { + else if (measuredType == "Joints Torque") { m_measuredType = wbt::ESTIMATE_JOINT_TORQUE; } else { - Log::getSingleton().error("Estimate not supported."); + Log::getSingleton().error("Measurement not supported."); + return false; + } + + // Get the DoFs + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); return false; } + const auto dofs = robotInterface->getConfiguration().getNumberOfDoFs(); // Initialize the size of the output vector - const unsigned dofs = getConfiguration().getNumberOfDoFs(); m_measurement.resize(dofs); // Retain the ControlBoardRemapper - if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { + if (!robotInterface->retainRemoteControlBoardRemapper()) { Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); return false; } @@ -112,9 +152,16 @@ bool GetMeasurement::initialize(const BlockInformation* blockInfo) bool GetMeasurement::terminate(const BlockInformation* blockInfo) { + // Get the RobotInterface + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); + return false; + } + // Release the RemoteControlBoardRemapper bool ok = true; - ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); + ok = ok && robotInterface->releaseRemoteControlBoardRemapper(); if (!ok) { Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); // Don't return false here. WBBlock::terminate must be called in any case @@ -125,12 +172,19 @@ bool GetMeasurement::terminate(const BlockInformation* blockInfo) bool GetMeasurement::output(const BlockInformation* blockInfo) { + // Get the RobotInterface + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); + return false; + } + bool ok; switch (m_measuredType) { case MEASUREMENT_JOINT_POS: { // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; - if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { + if (!robotInterface->getInterface(iEncoders) || !iEncoders) { Log::getSingleton().error("Failed to get IPidControl interface."); return false; } @@ -142,7 +196,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) case MEASUREMENT_JOINT_VEL: { // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; - if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { + if (!robotInterface->getInterface(iEncoders) || !iEncoders) { Log::getSingleton().error("Failed to get IEncoders interface."); return false; } @@ -154,7 +208,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) case MEASUREMENT_JOINT_ACC: { // Get the interface yarp::dev::IEncoders* iEncoders = nullptr; - if (!getRobotInterface()->getInterface(iEncoders) || !iEncoders) { + if (!robotInterface->getInterface(iEncoders) || !iEncoders) { Log::getSingleton().error("Failed to get IEncoders interface."); return false; } @@ -166,7 +220,7 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) case ESTIMATE_JOINT_TORQUE: { // Get the interface yarp::dev::ITorqueControl* iTorqueControl = nullptr; - if (!getRobotInterface()->getInterface(iTorqueControl) || !iTorqueControl) { + if (!robotInterface->getInterface(iTorqueControl) || !iTorqueControl) { Log::getSingleton().error("Failed to get ITorqueControl interface."); return false; } @@ -174,18 +228,22 @@ bool GetMeasurement::output(const BlockInformation* blockInfo) ok = iTorqueControl->getTorques(m_measurement.data()); break; } - default: - Log::getSingleton().error("Estimate type not recognized."); - return false; } if (!ok) { - Log::getSingleton().error("Failed to get estimate."); + Log::getSingleton().error("Failed to get measurement."); + return false; + } + + // Get the output signal + Signal output = blockInfo->getOutputPortSignal(0); + if (!output.getBuffer()) { + Log::getSingleton().error("Failed to get output signal."); return false; } - Signal signal = blockInfo->getOutputPortSignal(0); - signal.setBuffer(m_measurement.data(), blockInfo->getOutputPortWidth(0)); + // Fill the output buffer + output.setBuffer(m_measurement.data(), blockInfo->getOutputPortWidth(0)); return true; } diff --git a/toolbox/src/RealTimeSynchronizer.cpp b/toolbox/src/RealTimeSynchronizer.cpp index d1ce508fe..e772bc1ed 100644 --- a/toolbox/src/RealTimeSynchronizer.cpp +++ b/toolbox/src/RealTimeSynchronizer.cpp @@ -2,13 +2,15 @@ #include "BlockInformation.h" #include "Log.h" +#include #include using namespace wbt; const std::string RealTimeSynchronizer::ClassName = "RealTimeSynchronizer"; -const unsigned RealTimeSynchronizer::PARAM_PERIOD = 1; // Period +const unsigned PARAM_IDX_BIAS = Block::NumberOfParameters - 1; +const unsigned PARAM_IDX_PERIOD = PARAM_IDX_BIAS + 1; RealTimeSynchronizer::RealTimeSynchronizer() : m_period(0.01) @@ -18,7 +20,28 @@ RealTimeSynchronizer::RealTimeSynchronizer() unsigned RealTimeSynchronizer::numberOfParameters() { - return 1; + return Block::numberOfParameters() + 1; +} + +bool RealTimeSynchronizer::parseParameters(BlockInformation* blockInfo) +{ + if (!Block::parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse Block parameters."); + return false; + } + + ParameterMetadata paramMD_period(PARAM_DOUBLE, PARAM_IDX_PERIOD, 1, 1, "period"); + + bool ok = blockInfo->addParameterMetadata(paramMD_period); + + if (!ok) { + Log::getSingleton().error("Failed to store parameters metadata."); + return false; + } + + // Ask BlockInformation to parse the parameters using the metadata information + // and store them in a private class member + return blockInfo->parseParameters(parameters); } bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) @@ -48,24 +71,36 @@ bool RealTimeSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool RealTimeSynchronizer::initialize(const BlockInformation* blockInfo) +bool RealTimeSynchronizer::initialize(BlockInformation* blockInfo) { - if (!blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period)) { - Log::getSingleton().error("Failed to get input parametes."); + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); + return false; + } + + if (!parameters.getParameter("period", m_period)) { + Log::getSingleton().error("Failed to get parameter 'period' after its parsing."); return false; } - if (m_period < 0) { + if (m_period <= 0) { Log::getSingleton().error("Period must be greater than 0."); return false; } + yarp::os::Network::init(); + if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { + Log::getSingleton().error("YARP server wasn't found active!!"); + return false; + } + m_counter = 0; return true; } bool RealTimeSynchronizer::terminate(const BlockInformation* blockInfo) { + yarp::os::Network::fini(); return true; } diff --git a/toolbox/src/SetReferences.cpp b/toolbox/src/SetReferences.cpp index 84e38a5d5..a6cdb8cbc 100644 --- a/toolbox/src/SetReferences.cpp +++ b/toolbox/src/SetReferences.cpp @@ -1,9 +1,9 @@ #include "SetReferences.h" + #include "BlockInformation.h" #include "Log.h" #include "RobotInterface.h" #include "Signal.h" - #include #define _USE_MATH_DEFINES @@ -13,6 +13,10 @@ using namespace wbt; const std::string SetReferences::ClassName = "SetReferences"; +const unsigned PARAM_IDX_BIAS = WBBlock::NumberOfParameters - 1; +const unsigned PARAM_IDX_CTRL_TYPE = PARAM_IDX_BIAS + 1; +const unsigned PARAM_IDX_REF_SPEED = PARAM_IDX_BIAS + 2; + SetReferences::SetReferences() : m_resetControlMode(true) , m_refSpeed(0) @@ -36,6 +40,28 @@ unsigned SetReferences::numberOfParameters() return WBBlock::numberOfParameters() + 2; } +bool SetReferences::parseParameters(BlockInformation* blockInfo) +{ + // Gather the parameters + // ===================== + + // Set parameters metadata + ParameterMetadata paramMD_ctrlType(PARAM_STRING, PARAM_IDX_CTRL_TYPE, 1, 1, "CtrlType"); + ParameterMetadata paramMD_refSpeed(PARAM_DOUBLE, PARAM_IDX_REF_SPEED, 1, 1, "RefSpeed"); + + // Store the parameters metadata in the BlockInformation object + bool ok = true; + ok = ok && blockInfo->addParameterMetadata(paramMD_ctrlType); + ok = ok && blockInfo->addParameterMetadata(paramMD_refSpeed); + + if (!ok) { + Log::getSingleton().error("Failed to store parameters metadata."); + return false; + } + + return blockInfo->parseParameters(parameters); +} + bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) { // Memory allocation / Saving data not allowed here @@ -43,8 +69,6 @@ bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) if (!WBBlock::configureSizeAndPorts(blockInfo)) return false; - const unsigned dofs = getConfiguration().getNumberOfDoFs(); - // INPUTS // ====== // @@ -57,6 +81,14 @@ bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) return false; } + // Get the DoFs + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); + return false; + } + const auto dofs = robotInterface->getConfiguration().getNumberOfDoFs(); + // Size and type bool success = blockInfo->setInputPortVectorSize(0, dofs); blockInfo->setInputPortType(0, PortDataTypeDouble); @@ -80,27 +112,48 @@ bool SetReferences::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool SetReferences::initialize(const BlockInformation* blockInfo) +bool SetReferences::initialize(BlockInformation* blockInfo) { - if (!WBBlock::initialize(blockInfo)) { + if (!WBBlock::initialize(blockInfo)) + return false; + + // INPUT PARAMETERS + // ================ + + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); return false; } - // Reading the control type std::string controlType; - if (!blockInfo->getStringParameterAtIndex(WBBlock::numberOfParameters() + 1, controlType)) { + if (!parameters.getParameter("CtrlType", controlType)) { Log::getSingleton().error("Could not read control type parameter."); return false; } - // Reading the refSpeed - if (!blockInfo->getScalarParameterAtIndex(WBBlock::numberOfParameters() + 2, m_refSpeed)) { + if (!parameters.getParameter("RefSpeed", m_refSpeed)) { Log::getSingleton().error("Could not read reference speed parameter."); return false; } - // Initialize the std::vectors - const unsigned dofs = getConfiguration().getNumberOfDoFs(); + // PRIVATE MEMBERS + // =============== + + // Get the DoFs + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); + return false; + } + const auto dofs = robotInterface->getConfiguration().getNumberOfDoFs(); + + // Retain the ControlBoardRemapper + if (!robotInterface->retainRemoteControlBoardRemapper()) { + Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); + return false; + } + + // Initialize the size of std::vectors m_controlModes.assign(dofs, VOCAB_CM_UNKNOWN); // IControlMode.h @@ -134,7 +187,7 @@ bool SetReferences::initialize(const BlockInformation* blockInfo) if (controlType == "Position") { // Get the interface yarp::dev::IPositionControl* interface = nullptr; - if (!getRobotInterface()->getInterface(interface) || !interface) { + if (!robotInterface->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IPositionControl interface."); return false; } @@ -146,12 +199,6 @@ bool SetReferences::initialize(const BlockInformation* blockInfo) } } - // Retain the ControlBoardRemapper - if (!getRobotInterface()->retainRemoteControlBoardRemapper()) { - Log::getSingleton().error("Couldn't retain the RemoteControlBoardRemapper."); - return false; - } - m_resetControlMode = true; return true; } @@ -161,16 +208,25 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) using namespace yarp::dev; bool ok = true; - // Get the interface + // Get the RobotInterface + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("Couldn't get RobotInterface."); + return false; + } + + // Get the DoFs + const auto dofs = robotInterface->getConfiguration().getNumberOfDoFs(); + + // Get the IControlMode2 interface IControlMode2* icmd2 = nullptr; - ok = ok && getRobotInterface()->getInterface(icmd2); + ok = ok && robotInterface->getInterface(icmd2); if (!ok || !icmd2) { Log::getSingleton().error("Failed to get the IControlMode2 interface."); // Don't return false here. WBBlock::terminate must be called in any case } // Set all the controlledJoints VOCAB_CM_POSITION - const unsigned dofs = getConfiguration().getNumberOfDoFs(); m_controlModes.assign(dofs, VOCAB_CM_POSITION); ok = ok && icmd2->setControlModes(m_controlModes.data()); @@ -180,7 +236,7 @@ bool SetReferences::terminate(const BlockInformation* blockInfo) } // Release the RemoteControlBoardRemapper - ok = ok && getRobotInterface()->releaseRemoteControlBoardRemapper(); + ok = ok && robotInterface->releaseRemoteControlBoardRemapper(); if (!ok) { Log::getSingleton().error("Failed to release the RemoteControlBoardRemapper."); // Don't return false here. WBBlock::terminate must be called in any case @@ -198,9 +254,12 @@ bool SetReferences::initializeInitialConditions(const BlockInformation* /*blockI // and in this case the control mode should be set. // // It is worth noting that this toolbox disables parameters to be tunable for - // all the blocks + // all the blocks. + // Set again the control mode on the first output() call after the new enabling + // of the block m_resetControlMode = true; + return true; } @@ -208,12 +267,19 @@ bool SetReferences::output(const BlockInformation* blockInfo) { using namespace yarp::dev; + // Get the RobotInterface + const auto robotInterface = blockInfo->getRobotInterface().lock(); + if (!robotInterface) { + Log::getSingleton().error("Couldn't get RobotInterface."); + return false; + } + // Set the control mode at the first run if (m_resetControlMode) { m_resetControlMode = false; // Get the interface IControlMode2* icmd2 = nullptr; - if (!getRobotInterface()->getInterface(icmd2) || !icmd2) { + if (!robotInterface->getInterface(icmd2) || !icmd2) { Log::getSingleton().error("Failed to get the IControlMode2 interface."); return false; } @@ -244,7 +310,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) case VOCAB_CM_POSITION: { // Get the interface IPositionControl* interface = nullptr; - if (!getRobotInterface()->getInterface(interface) || !interface) { + if (!robotInterface->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IPositionControl interface."); return false; } @@ -257,7 +323,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) case VOCAB_CM_POSITION_DIRECT: { // Get the interface IPositionDirect* interface = nullptr; - if (!getRobotInterface()->getInterface(interface) || !interface) { + if (!robotInterface->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IPositionDirect interface."); return false; } @@ -270,7 +336,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) case VOCAB_CM_VELOCITY: { // Get the interface IVelocityControl* interface = nullptr; - if (!getRobotInterface()->getInterface(interface) || !interface) { + if (!robotInterface->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IVelocityControl interface."); return false; } @@ -283,7 +349,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) case VOCAB_CM_TORQUE: { // Get the interface ITorqueControl* interface = nullptr; - if (!getRobotInterface()->getInterface(interface) || !interface) { + if (!robotInterface->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get ITorqueControl interface."); return false; } @@ -294,7 +360,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) case VOCAB_CM_PWM: { // Get the interface IPWMControl* interface = nullptr; - if (!getRobotInterface()->getInterface(interface) || !interface) { + if (!robotInterface->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get IPWMControl interface."); return false; } @@ -305,7 +371,7 @@ bool SetReferences::output(const BlockInformation* blockInfo) case VOCAB_CM_CURRENT: { // Get the interface ICurrentControl* interface = nullptr; - if (!getRobotInterface()->getInterface(interface) || !interface) { + if (!robotInterface->getInterface(interface) || !interface) { Log::getSingleton().error("Failed to get ICurrentControl interface."); return false; } diff --git a/toolbox/src/SimulatorSynchronizer.cpp b/toolbox/src/SimulatorSynchronizer.cpp index bbf57a999..0567b9347 100644 --- a/toolbox/src/SimulatorSynchronizer.cpp +++ b/toolbox/src/SimulatorSynchronizer.cpp @@ -3,11 +3,10 @@ #include "Log.h" #include "thrift/ClockServer.h" +#include #include #include -#include - using namespace wbt; struct SimulatorSynchronizer::RPCData @@ -26,9 +25,10 @@ struct SimulatorSynchronizer::RPCData const std::string SimulatorSynchronizer::ClassName = "SimulatorSynchronizer"; -const unsigned SimulatorSynchronizer::PARAM_PERIOD = 1; -const unsigned SimulatorSynchronizer::PARAM_GZCLK_PORT = 2; -const unsigned SimulatorSynchronizer::PARAM_RPC_PORT = 3; +const unsigned PARAM_IDX_BIAS = Block::NumberOfParameters - 1; +const unsigned PARAM_IDX_PERIOD = PARAM_IDX_BIAS + 1; +const unsigned PARAM_IDX_GZCLK_PORT = PARAM_IDX_BIAS + 2; +const unsigned PARAM_IDX_RPC_PORT = PARAM_IDX_BIAS + 3; SimulatorSynchronizer::SimulatorSynchronizer() : m_period(0.01) @@ -37,7 +37,7 @@ SimulatorSynchronizer::SimulatorSynchronizer() unsigned SimulatorSynchronizer::numberOfParameters() { - return 3; + return Block::numberOfParameters() + 3; } std::vector SimulatorSynchronizer::additionalBlockOptions() @@ -45,6 +45,36 @@ std::vector SimulatorSynchronizer::additionalBlockOptions() return std::vector(1, wbt::BlockOptionPrioritizeOrder); } +bool SimulatorSynchronizer::parseParameters(BlockInformation* blockInfo) +{ + if (!Block::parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse Block parameters."); + return false; + } + + // Gather the parameters + // ===================== + + // Set parameters metadata + ParameterMetadata paramMD_period(PARAM_DOUBLE, PARAM_IDX_PERIOD, 1, 1, "Period"); + ParameterMetadata paramMD_gzclkPort( + PARAM_STRING, PARAM_IDX_GZCLK_PORT, 1, 1, "GazeboClockPort"); + ParameterMetadata paramMD_rpcPort(PARAM_STRING, PARAM_IDX_RPC_PORT, 1, 1, "RpcPort"); + + // Store the parameters metadata in the BlockInformation object + bool ok = true; + ok = ok && blockInfo->addParameterMetadata(paramMD_period); + ok = ok && blockInfo->addParameterMetadata(paramMD_gzclkPort); + ok = ok && blockInfo->addParameterMetadata(paramMD_rpcPort); + + if (!ok) { + Log::getSingleton().error("Failed to store parameters metadata."); + return false; + } + + return blockInfo->parseParameters(parameters); +} + bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) { // INPUTS @@ -72,15 +102,20 @@ bool SimulatorSynchronizer::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool SimulatorSynchronizer::initialize(const BlockInformation* blockInfo) +bool SimulatorSynchronizer::initialize(BlockInformation* blockInfo) { + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); + return false; + } + std::string serverPortName; std::string clientPortName; bool ok = true; - ok = ok && blockInfo->getScalarParameterAtIndex(PARAM_PERIOD, m_period); - ok = ok && blockInfo->getStringParameterAtIndex(PARAM_GZCLK_PORT, serverPortName); - ok = ok && blockInfo->getStringParameterAtIndex(PARAM_RPC_PORT, clientPortName); + ok = ok && parameters.getParameter("Period", m_period); + ok = ok && parameters.getParameter("GazeboClockPort", serverPortName); + ok = ok && parameters.getParameter("RpcPort", clientPortName); if (!ok) { Log::getSingleton().error("Error reading RPC parameters."); diff --git a/toolbox/src/YarpClock.cpp b/toolbox/src/YarpClock.cpp index ba860a232..c16cf1ff2 100644 --- a/toolbox/src/YarpClock.cpp +++ b/toolbox/src/YarpClock.cpp @@ -1,4 +1,5 @@ #include "YarpClock.h" + #include "BlockInformation.h" #include "Log.h" #include "Signal.h" @@ -12,7 +13,7 @@ const std::string YarpClock::ClassName = "YarpClock"; unsigned YarpClock::numberOfParameters() { - return 0; + return Block::numberOfParameters(); } bool YarpClock::configureSizeAndPorts(BlockInformation* blockInfo) @@ -45,8 +46,13 @@ bool YarpClock::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool YarpClock::initialize(const BlockInformation* blockInfo) +bool YarpClock::initialize(BlockInformation* blockInfo) { + if (!parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); + return false; + } + yarp::os::Network::init(); if (!yarp::os::Network::initialized() || !yarp::os::Network::checkNetwork(5.0)) { diff --git a/toolbox/src/base/Block.cpp b/toolbox/src/base/Block.cpp index e4bed056b..6076813c3 100644 --- a/toolbox/src/base/Block.cpp +++ b/toolbox/src/base/Block.cpp @@ -1,9 +1,15 @@ #include "Block.h" -#include "toolbox.h" +#include "BlockInformation.h" using namespace wbt; -Block::~Block() {} +const unsigned Block::NumberOfParameters = 1; + +unsigned Block::numberOfParameters() +{ + return Block::NumberOfParameters; +} + std::vector Block::additionalBlockOptions() { return std::vector(); @@ -19,11 +25,17 @@ bool Block::checkParameters(const BlockInformation* /*blockInfo*/) return true; } +bool Block::parseParameters(BlockInformation* blockInfo) +{ + ParameterMetadata paramMD_className(PARAM_STRING, 0, 1, 1, "className"); + blockInfo->addParameterMetadata(paramMD_className); + return blockInfo->parseParameters(parameters); +} + unsigned Block::numberOfDiscreteStates() { return 0; } - unsigned Block::numberOfContinuousStates() { return 0; @@ -43,3 +55,9 @@ bool Block::initializeInitialConditions(const BlockInformation* /*blockInfo*/) { return true; } + +bool Block::getParameters(wbt::Parameters& params) const +{ + params = parameters; + return true; +} diff --git a/toolbox/src/base/BlockInformation.cpp b/toolbox/src/base/BlockInformation.cpp index 5e52eb899..417e5a1e9 100644 --- a/toolbox/src/base/BlockInformation.cpp +++ b/toolbox/src/base/BlockInformation.cpp @@ -1,4 +1,5 @@ #include "BlockInformation.h" +#include "Parameters.h" const std::string wbt::BlockOptionPrioritizeOrder = "wbt.BlockOptionPrioritizeOrder"; diff --git a/toolbox/src/base/CoderBlockInformation.cpp b/toolbox/src/base/CoderBlockInformation.cpp new file mode 100644 index 000000000..01a8286bf --- /dev/null +++ b/toolbox/src/base/CoderBlockInformation.cpp @@ -0,0 +1,266 @@ +#include "CoderBlockInformation.h" +#include "Signal.h" +#include "ToolboxSingleton.h" + +#include +#include +#include + +using namespace wbt; + +// BLOCK OPTIONS METHODS +// ===================== + +bool CoderBlockInformation::optionFromKey(const std::string& key, double& option) const +{ + return true; +} + +// PARAMETERS METHODS +// ================== + +// PORT INFORMATION SETTERS +// ======================== + +bool CoderBlockInformation::setNumberOfInputPorts(unsigned numberOfPorts) +{ + return true; +} + +bool CoderBlockInformation::setNumberOfOutputPorts(unsigned numberOfPorts) +{ + return true; +} + +bool CoderBlockInformation::setInputPortVectorSize(unsigned portNumber, int portSize) +{ + m_inputSignalSize[portNumber] = {1, portSize}; + return true; +} + +bool CoderBlockInformation::setInputPortMatrixSize(unsigned portNumber, int rows, int columns) +{ + m_inputSignalSize[portNumber] = {rows, columns}; + return true; +} + +bool CoderBlockInformation::setOutputPortVectorSize(unsigned portNumber, int portSize) +{ + m_outputSignalSize[portNumber] = {1, portSize}; + return true; +} + +bool CoderBlockInformation::setOutputPortMatrixSize(unsigned portNumber, int rows, int columns) +{ + m_outputSignalSize[portNumber] = {rows, columns}; + return true; +} + +bool CoderBlockInformation::setInputPortType(unsigned portNumber, PortDataType portType) +{ + return false; +} + +bool CoderBlockInformation::setOutputPortType(unsigned portNumber, PortDataType portType) +{ + return false; +} + +// PORT INFORMATION GETTERS +// ======================== + +unsigned CoderBlockInformation::getInputPortWidth(unsigned portNumber) const +{ + if (m_inputSignalSize.find(portNumber) == m_inputSignalSize.end()) { + return 0; + } + + return static_cast(m_inputSignalSize.at(portNumber).second); +} + +unsigned CoderBlockInformation::getOutputPortWidth(unsigned portNumber) const +{ + if (m_outputSignalSize.find(portNumber) == m_outputSignalSize.end()) { + return 0; + } + + return static_cast(m_outputSignalSize.at(portNumber).second); +} + +wbt::Signal CoderBlockInformation::getInputPortSignal(unsigned portNumber, int portWidth) const +{ + if (m_inputSignals.find(portNumber) == m_inputSignals.end()) { + 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?? + if (portWidth != -1 && m_inputSignals.at(portNumber).getWidth() != portWidth) { + return {}; + } + + return m_inputSignals.at(portNumber); +} + +wbt::Signal CoderBlockInformation::getOutputPortSignal(unsigned portNumber, int portWidth) const +{ + // TODO: verify if all blocks check if the pointer contained in the returned signal is not + // nullptr (e.g. DiscreteFilter doesn't do it) + + if (m_outputSignals.find(portNumber) == m_outputSignals.end()) { + return {}; + } + + // TODO: how to handle DYNAMIC_SIZEd signals? + // if (m_outputSignals.at(portNumber).getWidth() != portWidth) { + // return wbt::Signal(); + // } + + return m_outputSignals.at(portNumber); +} + +bool CoderBlockInformation::addParameterMetadata(const wbt::ParameterMetadata& paramMD) +{ + m_paramsMetadata.push_back(paramMD); + return true; +} + +bool CoderBlockInformation::parseParameters(wbt::Parameters& parameters) +{ + if (m_parametersFromRTW.getNumberOfParameters() == 0) { + return false; + } + + for (wbt::ParameterMetadata md : m_paramsMetadata) { + // Check that all the parameters that are parsed have already been stored from the coder + if (!m_parametersFromRTW.existName(md.name)) { + return false; + } + // Check that the metadata match + // TODO: The metadata passed from wbt::Block might contain dynamically sized cols / rows, + // but in the RTW those values are substituted by the real size. Skip for now this + // check in this case. + if (!(md.rows == -1 || md.cols == -1) + && md != m_parametersFromRTW.getParameterMetadata(md.name)) { + return false; + } + } + + // Configuration configuration; + // if (!fillConfiguration(configuration, parameters)) { + // //Log::getSingleton().error("Failed to fill the configuration with input + // parameters."); return false; + // } + // if (!configuration.isValid()) { + // //Log::getSingleton().error("Parsed Configuration object is not valid."); + // return false; + // } + // // Save the name of the Configuration block which the processed WBBlock refers to + // m_confBlockName = configuration.getConfKey(); + // // Insert the configuration into the Toolbox Singleton + // if (!ToolboxSingleton::sharedInstance().storeConfiguration(m_confBlockName, + // configuration)) { + //// Log::getSingleton().error("Failed to store the given configuration in the + /// ToolboxSingleton."); + // return false; + // } + // } + + // TODO: instead of returning only the parameters contained in the populated m_paramsMetadata, + // return all the stored parameters. The previous checks assure that the asked parameters + // are present. + // TODO: clear the m_paramsMetadata vector? + parameters = m_parametersFromRTW; + return true; +} + +bool CoderBlockInformation::storeRTWParameters(const Parameters& parameters) +{ + if (parameters.getNumberOfParameters() == 0) { + return false; + } + + m_parametersFromRTW = parameters; + + // This code is shared with SimulinkBlockInformation::parseParameters + // + // Check if the parameters object contains all the information for creating a + // Configuration object. + if (Parameters::containConfigurationData(parameters)) { + if (!ToolboxSingleton::sharedInstance().storeConfiguration(parameters)) { + return false; + } + // Save the name of the Configuration block which the processed WBBlock refers to + if (!parameters.getParameter("ConfBlockName", m_confBlockName)) { + // TODO + return false; + } + } + + // // For WBBlocks, extract the name of the Configuration block which it refers to + // if (parameters.existName("ConfBlockName")) { + // if(!parameters.getParameter("ConfBlockName", m_confBlockName)) { + // return false; + // } + // } + + return true; +} + +bool CoderBlockInformation::setInputSignal(unsigned portNumber, void* address, int portWidth) +{ + if (m_inputSignals.find(portNumber) != m_inputSignals.end()) { + return false; + } + + bool isConst = true; + m_inputSignals.emplace(std::piecewise_construct, + std::forward_as_tuple(portNumber), + std::forward_as_tuple(CONTIGUOUS_ZEROCOPY, PortDataTypeDouble, isConst)); + + m_inputSignals[portNumber].setWidth(portWidth); + return m_inputSignals[portNumber].initializeBufferFromContiguousZeroCopy(address); +} + +bool CoderBlockInformation::setOutputSignal(unsigned portNumber, void* address, int portWidth) +{ + if (m_outputSignals.find(portNumber) != m_outputSignals.end()) { + return false; + } + + if (!address) { + return false; + } + + bool isConst = false; + m_outputSignals.emplace( + std::piecewise_construct, + std::forward_as_tuple(portNumber), + std::forward_as_tuple(CONTIGUOUS_ZEROCOPY, PortDataTypeDouble, isConst)); + + m_outputSignals[portNumber].setWidth(portWidth); + return m_outputSignals[portNumber].initializeBufferFromContiguousZeroCopy(address); +} + +std::weak_ptr CoderBlockInformation::getRobotInterface() const +{ + if (m_confBlockName.empty()) { + // Return an empty weak pointer + return std::weak_ptr(); + } + + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + return interface.getRobotInterface(m_confBlockName); +} + +std::weak_ptr CoderBlockInformation::getKinDynComputations() const +{ + if (m_confBlockName.empty()) { + // Return an empty weak pointer + return std::weak_ptr(); + } + + ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + return interface.getKinDynComputations(m_confBlockName); +} diff --git a/toolbox/src/base/Configuration.cpp b/toolbox/src/base/Configuration.cpp index a6f9d5ce3..988f70648 100644 --- a/toolbox/src/base/Configuration.cpp +++ b/toolbox/src/base/Configuration.cpp @@ -1,5 +1,4 @@ #include "Configuration.h" - #include using namespace wbt; diff --git a/toolbox/src/base/GeneratedCodeWrapper.cpp b/toolbox/src/base/GeneratedCodeWrapper.cpp new file mode 100644 index 000000000..e69de29bb diff --git a/toolbox/src/base/Parameter.cpp b/toolbox/src/base/Parameter.cpp new file mode 100644 index 000000000..82606f5f0 --- /dev/null +++ b/toolbox/src/base/Parameter.cpp @@ -0,0 +1,79 @@ +#include "Parameter.h" + +using namespace wbt; + +// ================== +// PARAMETER METADATA +// ================== + +// TODO: +// +// Alla lunga i blocchi WBBlock non avranno piu bisogno di avere la struct di +// WBToolboxConfig. Infatti quelle informazioni saranno contenute solo nel nuovo +// blocco YARP. +// +// Il nuovo blocco YARP implementera: +// Configuration, SetReference, GetMeasurement, SetLowLevelPID +// +// La classe WBBlock avra dei metodi fuori dall'interfaccia per settare il pointer +// ad un oggetto RobotInterface. (o solo KinDyn visto che serve solo il modello?) +// In Simulink l'oggetto vero sara contenuto dentro il blocco YARP. +// Nel Coder sara contenuto da qualche parte nel wrapper. +// + +const std::string defaultParamPrefix = "ParamIndex_"; + +ParameterMetadata::ParameterMetadata(const ParameterType& t, + const unsigned& paramIndex, + const int& paramRows, + const int& paramCols, + const std::string& paramName) + : type(t) + , index(paramIndex) + , rows(paramRows) + , cols(paramCols) + , name(paramName.empty() ? (defaultParamPrefix + std::to_string(index)) : paramName) +// , isStructField(paramIsStructField) +{} + +ParameterMetadata::ParameterMetadata(const ParameterMetadata& paramMD) + : type(paramMD.type) + , index(paramMD.index) + , rows(paramMD.rows) + , cols(paramMD.cols) + , name(paramMD.name.empty() ? (defaultParamPrefix + std::to_string(index)) : paramMD.name) +// , isStructField(paramMD.isStructField) +{} + +ParameterMetadata::ParameterMetadata(ParameterMetadata&& other) + : type(other.type) + , index(other.index) + , rows(other.rows) + , cols(other.cols) + , name(other.name.empty() ? (defaultParamPrefix + std::to_string(index)) : other.name) +// , isStructField(other.isStructField) +{} + +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) +{ + 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/src/base/Parameters.cpp b/toolbox/src/base/Parameters.cpp new file mode 100644 index 000000000..09f89f2c8 --- /dev/null +++ b/toolbox/src/base/Parameters.cpp @@ -0,0 +1,1461 @@ +#include "Parameters.h" +#include +#include + +using namespace wbt; + +// ========== +// PARAMETERS +// ========== + +// Template and template specializations declarations +// 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); template<> void +// convertStdVector(const std::vector& input, std::vector& output); template<> +// void convertStdVector(const std::vector& input, std::vector& output); +// template<> std::vector convertStdVector(const std::vector& input); +// template<> std::vector convertStdVector(const std::vector& input); +// template<> std::vector convertStdVector(const std::vector& input); +// template<> std::vector convertStdVector(const std::vector& input); +// std::vector convertStdVector(const std::vector& input); +// std::vector convertStdVector(const std::vector& input); +// std::vector convertStdVector(const std::vector& input); +// std::vector convertStdVector(const std::vector& input); + +Parameters::ParamName Parameters::getParamName(const Parameters::ParamIndex& index) const +{ + if (m_indexToName.find(index) == m_indexToName.end()) { + return PARAM_INVALID_NAME; + } + + return m_indexToName.at(index); +} + +Parameters::ParamIndex Parameters::getParamIndex(const Parameters::ParamName& name) const +{ + if (m_nameToIndex.find(name) == m_nameToIndex.end()) { + return PARAM_INVALID_INDEX; + } + + return m_nameToIndex.at(name); +} + +// Scalar parameters +// ================= + +// Specialize the template for std::string +template <> +bool wbt::Parameters::storeParameter(const std::string& param, + const wbt::ParameterMetadata& paramMetadata) +{ + if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { + return false; + } + + if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { + return false; + } + + switch (paramMetadata.type) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: + m_paramsInt.emplace( + std::make_pair(paramMetadata.name, ParameterInt(std::stoi(param), paramMetadata))); + break; + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: + m_paramsBool.emplace( + std::make_pair(paramMetadata.name, + ParameterBool(static_cast(std::stoi(param)), paramMetadata))); + break; + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: + m_paramsDouble.emplace(std::make_pair( + paramMetadata.name, ParameterDouble(std::stod(param), paramMetadata))); + break; + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: + m_paramsString.emplace( + std::make_pair(paramMetadata.name, ParameterString(param, paramMetadata))); + break; + } + + m_nameToType[paramMetadata.name] = paramMetadata.type; + m_nameToIndex[paramMetadata.name] = paramMetadata.index; + m_indexToName[paramMetadata.index] = paramMetadata.name; + + return true; +} + +// bool Parameters::storeParameter(const int& param, const wbt::ParameterMetadata& paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// m_paramsInt.emplace( +// std::make_pair(paramMetadata.name, +// ParameterInt(static_cast(param), paramMetadata))); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// m_paramsBool.emplace( +// std::make_pair(paramMetadata.name, +// ParameterBool(static_cast(param), paramMetadata))); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// m_paramsDouble.emplace( +// std::make_pair(paramMetadata.name, +// ParameterDouble(static_cast(param), paramMetadata))); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// m_paramsString.emplace( +// std::make_pair(paramMetadata.name, +// ParameterString(std::to_string(param), paramMetadata))); +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// bool Parameters::storeParameter(const bool& param, const wbt::ParameterMetadata& paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// m_paramsInt.emplace( +// std::make_pair(paramMetadata.name, +// ParameterInt(static_cast(param), paramMetadata))); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// m_paramsBool.emplace( +// std::make_pair(paramMetadata.name, +// ParameterBool(static_cast(param), paramMetadata))); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// m_paramsDouble.emplace( +// std::make_pair(paramMetadata.name, +// ParameterDouble(static_cast(param), paramMetadata))); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// m_paramsString.emplace( +// std::make_pair(paramMetadata.name, +// ParameterString(std::to_string(param), paramMetadata))); +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// bool Parameters::storeParameter(const double& param, const wbt::ParameterMetadata& paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// m_paramsInt.emplace( +// std::make_pair(paramMetadata.name, +// ParameterInt(static_cast(param), paramMetadata))); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// m_paramsBool.emplace( +// std::make_pair(paramMetadata.name, +// ParameterBool(static_cast(param), paramMetadata))); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// m_paramsDouble.emplace( +// std::make_pair(paramMetadata.name, +// ParameterDouble(static_cast(param), paramMetadata))); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// m_paramsString.emplace( +// std::make_pair(paramMetadata.name, +// ParameterString(std::to_string(param), paramMetadata))); +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} +// bool Parameters::storeParameter(const std::string& param, const wbt::ParameterMetadata& +// paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != 1) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// m_paramsInt.emplace( +// std::make_pair(paramMetadata.name, +// ParameterInt(std::stoi(param), paramMetadata))); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// m_paramsBool.emplace( +// std::make_pair(paramMetadata.name, +// ParameterBool(static_cast(std::stoi(param)), paramMetadata))); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// m_paramsDouble.emplace( +// std::make_pair(paramMetadata.name, +// ParameterDouble(std::stod(param), paramMetadata))); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// m_paramsString.emplace( +// std::make_pair(paramMetadata.name, +// ParameterString(param, paramMetadata))); +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// TODO +// template void convertStdVector(const std::vector& input, +// std::vector& output); +template <> +void Parameters::convertStdVector(const std::vector& input, std::vector& output); +template <> +void Parameters::convertStdVector(const std::vector& input, std::vector& output); +template <> +void Parameters::convertStdVector(const std::vector& input, + std::vector& output); +template <> +void Parameters::convertStdVector(const std::vector& input, + std::vector& output); + +// Generic template + +template +void Parameters::convertStdVector(const std::vector& input, std::vector& output) +{ + output = std::vector(input.begin(), input.end()); +} + +// std::string -> numeric + +template <> +void Parameters::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); + }); + // for (auto element : input) { + // output.push_back(std::stoi(element)); + // } +} + +template <> +void Parameters::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)); + }); + // for (auto element : input) { + // output.push_back(static_cast(std::stoi(element))); + // } +} + +template <> +void Parameters::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); + }); + // for (auto element : input) { + // output.push_back(std::stod(element)); + // } +} + +// numeric -> std::string + +template <> +void Parameters::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); + }); + // std::transform(input.begin(), input.end(), output.begin(), std::to_string); + // for (auto element : input) { + // output.push_back(std::to_string(element)); + // } +} + +template <> +void Parameters::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); + }); + // for (auto element : input) { + // output.push_back(std::to_string(element)); + // } +} + +template <> +void Parameters::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); + }); + // for (auto element : input) { + // output.push_back(std::to_string(element)); + // } +} + +// std::string -> std::string + +template <> +void Parameters::convertStdVector(const std::vector& input, + std::vector& output) +{ + output = input; + return; +} + +// ============================ + +template +bool Parameters::storeVectorParameter(const std::vector& param, + const wbt::ParameterMetadata& paramMetadata) +{ + if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { + return false; + } + + if (paramMetadata.rows != 1 && paramMetadata.cols != param.size()) { + return false; + } + + // TODO: remove {} + switch (paramMetadata.type) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: { + std::vector paramInt(param.size()); + convertStdVector(param, paramInt); + m_paramsInt.emplace( + std::make_pair(paramMetadata.name, ParameterInt(paramInt, paramMetadata))); + break; + } + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: { + std::vector paramBool(param.size()); + convertStdVector(param, paramBool); + m_paramsBool.emplace( + std::make_pair(paramMetadata.name, ParameterBool(paramBool, paramMetadata))); + break; + } + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: { + std::vector paramDouble(param.size()); + convertStdVector(param, paramDouble); + m_paramsDouble.emplace( + std::make_pair(paramMetadata.name, ParameterDouble(paramDouble, paramMetadata))); + break; + } + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: { + std::vector paramString(param.size()); + convertStdVector(param, paramString); + m_paramsString.emplace( + std::make_pair(paramMetadata.name, ParameterString(paramString, paramMetadata))); + break; + } + } + + m_nameToType[paramMetadata.name] = paramMetadata.type; + m_nameToIndex[paramMetadata.name] = paramMetadata.index; + m_indexToName[paramMetadata.index] = paramMetadata.name; + + return true; +} + +// Templated Vector Parameters +template <> +bool Parameters::storeParameter>(const std::vector& param, + const wbt::ParameterMetadata& paramMetadata) +{ + return storeVectorParameter(param, paramMetadata); +} + +template <> +bool Parameters::storeParameter>(const std::vector& param, + const wbt::ParameterMetadata& paramMetadata) +{ + return storeVectorParameter(param, paramMetadata); +} + +template <> +bool Parameters::storeParameter>(const std::vector& param, + const wbt::ParameterMetadata& paramMetadata) +{ + return storeVectorParameter(param, paramMetadata); +} + +template <> +bool Parameters::storeParameter>( + const std::vector& param, const wbt::ParameterMetadata& paramMetadata) +{ + return storeVectorParameter(param, paramMetadata); +} + +// Vector parameters +// bool Parameters::storeParameter(const ParamVectorInt& param, const wbt::ParameterMetadata& +// paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != param.size()) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// m_paramsInt.emplace( +// std::make_pair(paramMetadata.name, +// ParameterInt(param, paramMetadata))); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// // TODO +// return false; +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// // TODO +// return false; +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// // TODO +// return false; +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// bool Parameters::storeParameter(const ParamVectorBool& param, const wbt::ParameterMetadata& +// paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != param.size()) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// // TODO +// return false; +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// m_paramsBool.emplace( +// std::make_pair(paramMetadata.name, +// ParameterBool(param, paramMetadata))); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// // TODO +// return false; +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// // TODO +// return false; +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// bool Parameters::storeParameter(const ParamVectorDouble& param, const wbt::ParameterMetadata& +// paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != param.size()) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// // TODO +// return false; +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// // TODO +// return false; +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// m_paramsDouble.emplace( +// std::make_pair(paramMetadata.name, +// ParameterDouble(param, paramMetadata))); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// // TODO +// return false; +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// bool Parameters::storeParameter(const ParamVectorString& param, const wbt::ParameterMetadata& +// paramMetadata) +//{ +// if (existName(paramMetadata.name) || existName(paramMetadata.name, paramMetadata.type)) { +// return false; +// } + +// if (paramMetadata.rows != 1 && paramMetadata.cols != param.size()) { +// return false; +// } + +// switch (paramMetadata.type) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// // TODO +// return false; +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// // TODO +// return false; +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// // TODO +// return false; +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// m_paramsString.emplace( +// std::make_pair(paramMetadata.name, +// ParameterString(param, paramMetadata))); +// break; +// } + +// m_nameToType[paramMetadata.name] = paramMetadata.type; +// m_nameToIndex[paramMetadata.name] = paramMetadata.index; +// m_indexToName[paramMetadata.index] = paramMetadata.name; + +// return true; +//} + +// bool Parameters::getParameter(const ParamName& name, int& param) const +//{ +// // The two checks are requires because before using the at() method we need to be +// // sure that the entry addressed by name does exist +// // if (!existIndex(index) || !existIndex(index, m_indexToParamInfo.at(index).second)) { +// // return false; +// // } +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// if (!m_paramsInt.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsInt.at(name).getScalarParameter()); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// if (!m_paramsBool.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsBool.at(name).getScalarParameter()); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// if (!m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsDouble.at(name).getScalarParameter()); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// if (!m_paramsString.at(name).isScalar()) { +// return false; +// } +// param = std::stoi(m_paramsString.at(name).getScalarParameter()); +// break; +// } +// return true; +//} + +// bool Parameters::getParameter(const ParamName& name, bool& param) const +//{ +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// if (!m_paramsInt.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsInt.at(name).getScalarParameter()); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// if (!m_paramsBool.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsBool.at(name).getScalarParameter()); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// if (!m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsDouble.at(name).getScalarParameter()); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// if (!m_paramsString.at(name).isScalar()) { +// return false; +// } +// param = static_cast(std::stoi(m_paramsString.at(name).getScalarParameter())); +// break; +// } +// return true; +//} + +// bool Parameters::getParameter(const ParamName& name, double& param) const +//{ +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// if (!m_paramsInt.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsInt.at(name).getScalarParameter()); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// if (!m_paramsBool.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsBool.at(name).getScalarParameter()); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// if (!m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// param = static_cast(m_paramsDouble.at(name).getScalarParameter()); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// if (!m_paramsString.at(name).isScalar()) { +// return false; +// } +// // TODO: template and static_cast(stod...) for everything +// param = std::stod(m_paramsString.at(name).getScalarParameter()); +// break; +// } +// return true; +//} + +// bool Parameters::getParameter(const ParamName& name, std::string& param) const +//{ +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: +// if (!m_paramsInt.at(name).isScalar()) { +// return false; +// } +// param = std::to_string(m_paramsInt.at(name).getScalarParameter()); +// break; +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: +// if (!m_paramsBool.at(name).isScalar()) { +// return false; +// } +// param = std::to_string(m_paramsBool.at(name).getScalarParameter()); +// break; +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: +// if (!m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// param = std::to_string(m_paramsDouble.at(name).getScalarParameter()); +// break; +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: +// if (!m_paramsString.at(name).isScalar()) { +// return false; +// } +// param = m_paramsString.at(name).getScalarParameter(); +// break; +// } +// return true; +//} + +template <> +bool Parameters::getParameter(const ParamName& name, std::string& param) const +{ + if (!existName(name) || !existName(name, m_nameToType.at(name))) { + return false; + } + + switch (m_nameToType.at(name)) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: + if (!m_paramsInt.at(name).isScalar()) { + return false; + } + param = std::to_string(m_paramsInt.at(name).getScalarParameter()); + break; + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: + if (!m_paramsBool.at(name).isScalar()) { + return false; + } + param = std::to_string(m_paramsBool.at(name).getScalarParameter()); + break; + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: + if (!m_paramsDouble.at(name).isScalar()) { + return false; + } + param = std::to_string(m_paramsDouble.at(name).getScalarParameter()); + break; + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: + if (!m_paramsString.at(name).isScalar()) { + return false; + } + param = m_paramsString.at(name).getScalarParameter(); + break; + } + return true; +} + +// template<> bool wbt::Parameters::getParameter(const wbt::Parameters::ParamName& name, +// std::vector& param) const +//{ + +//} + +// template<> bool wbt::Parameters::getParameter>(const wbt::Parameters::ParamName& +// name, +// std::vector& param) const +//{ + +//} + +// template<> bool wbt::Parameters::getParameter>(const +// wbt::Parameters::ParamName& name, +// std::vector& param) const +//{ + +//} + +// template<> bool wbt::Parameters::getParameter>(const +// wbt::Parameters::ParamName& name, +// std::vector& param) +// const +//{ + +//} + +// template<> bool wbt::Parameters::getParameter>(const +// wbt::Parameters::ParamName& name, +// std::vector& +// param) const +//{ + +//} + +// CELLS, VECTORS, MATRICES +// ------------------------ + +// bool Parameters::getParameter(const ParamName& name, ParamVectorInt& param) const +//{ +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// param.clear(); + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: { +// if (m_paramsInt.at(name).isScalar()) { +// return false; +// } +// param = m_paramsInt.at(name).getVectorParameter(); +// break; +// } +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: { +// if (m_paramsBool.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsBool.at(name).getVectorParameter(); +// param.assign(vector.begin(), vector.end()); +// break; +// } +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: { +// if (m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsDouble.at(name).getVectorParameter(); +// param.assign(vector.begin(), vector.end()); +// break; +// } +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: { +// if (m_paramsString.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsString.at(name).getVectorParameter(); +// param.reserve(vector.size()); +// for (auto i = 0; i < vector.size(); ++i) { +// param.push_back(std::stoi(vector[i])); +// } +// break; +// } +// } +// return true; +//} +// bool Parameters::getParameter(const ParamName& name, ParamVectorBool& param) const +//{ +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// param.clear(); + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: { +// if (m_paramsInt.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsInt.at(name).getVectorParameter(); +// param.assign(vector.begin(), vector.end()); +// break; +// } +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: { +// if (m_paramsBool.at(name).isScalar()) { +// return false; +// } +// param = m_paramsBool.at(name).getVectorParameter(); +// break; +// } +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: { +// if (m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsDouble.at(name).getVectorParameter(); +// param.assign(vector.begin(), vector.end()); +// break; +// } +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: { +// if (m_paramsString.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsString.at(name).getVectorParameter(); +// param.reserve(vector.size()); +// for (auto i = 0; i < vector.size(); ++i) { +// // TODO: be aware here +// param.push_back(static_cast(std::stoi(vector[i]))); +// } +// break; +// } +// } +// return true; +//} + +// bool Parameters::getParameter(const ParamName& name, ParamVectorDouble& param) const +//{ +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// param.clear(); + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: { +// if (m_paramsInt.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsInt.at(name).getVectorParameter(); +// param.assign(vector.begin(), vector.end()); +// break; +// } +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: { +// if (m_paramsBool.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsBool.at(name).getVectorParameter(); +// param.assign(vector.begin(), vector.end()); +// break; +// } +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: { +// if (m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// param = m_paramsDouble.at(name).getVectorParameter(); +// break; +// } +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: { +// if (m_paramsString.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsString.at(name).getVectorParameter(); +// param.reserve(vector.size()); +// for (auto i = 0; i < vector.size(); ++i) { +// // TODO: careful +// param.push_back(std::stod(vector[i])); +// } +// break; +// } +// } +// return true; +//} + +// bool Parameters::getParameter(const ParamName& name, ParamVectorString& param) const +//{ +// if (!existName(name) || !existName(name, m_nameToType.at(name))) { +// return false; +// } + +// param.clear(); + +// switch (m_nameToType.at(name)) { +// case PARAM_INT: +// case PARAM_CELL_INT: +// case PARAM_STRUCT_INT: +// case PARAM_STRUCT_CELL_INT: { +// if (m_paramsInt.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsInt.at(name).getVectorParameter(); +// param.reserve(vector.size()); +// for (auto i = 0; i < vector.size(); ++i) { +// param.push_back(std::to_string(vector[i])); +// } +// break; +// } +// case PARAM_BOOL: +// case PARAM_CELL_BOOL: +// case PARAM_STRUCT_BOOL: +// case PARAM_STRUCT_CELL_BOOL: { +// if (m_paramsBool.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsBool.at(name).getVectorParameter(); +// param.reserve(vector.size()); +// for (auto i = 0; i < vector.size(); ++i) { +// param.push_back(std::to_string(vector[i])); +// } +// break; +// } +// case PARAM_DOUBLE: +// case PARAM_CELL_DOUBLE: +// case PARAM_STRUCT_DOUBLE: +// case PARAM_STRUCT_CELL_DOUBLE: { +// if (m_paramsDouble.at(name).isScalar()) { +// return false; +// } +// const auto vector = m_paramsDouble.at(name).getVectorParameter(); +// param.reserve(vector.size()); +// for (auto i = 0; i < vector.size(); ++i) { +// param.push_back(std::to_string(vector[i])); +// } +// break; +// } +// case PARAM_STRING: +// case PARAM_CELL_STRING: +// case PARAM_STRUCT_STRING: +// case PARAM_STRUCT_CELL_STRING: { +// if (m_paramsString.at(name).isScalar()) { +// return false; +// } +// param = m_paramsString.at(name).getVectorParameter(); +// break; +// } +// } +// return true; +//} + +template <> +bool Parameters::getParameter>(const wbt::Parameters::ParamName& name, + std::vector& param) const +{ + if (!existName(name) || !existName(name, m_nameToType.at(name))) { + return false; + } + + param.clear(); + + switch (m_nameToType.at(name)) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: { + if (m_paramsInt.at(name).isScalar()) { + return false; + } + const auto vector = m_paramsInt.at(name).getVectorParameter(); + param.reserve(vector.size()); + for (auto element : vector) { + param.push_back(std::to_string(element)); + } + break; + } + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: { + if (m_paramsBool.at(name).isScalar()) { + return false; + } + const auto vector = m_paramsBool.at(name).getVectorParameter(); + param.reserve(vector.size()); + for (auto element : vector) { + param.push_back(std::to_string(element)); + } + break; + } + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: { + if (m_paramsDouble.at(name).isScalar()) { + return false; + } + const auto vector = m_paramsDouble.at(name).getVectorParameter(); + param.reserve(vector.size()); + for (auto element : vector) { + param.push_back(std::to_string(element)); + } + break; + } + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: { + if (m_paramsString.at(name).isScalar()) { + return false; + } + param = m_paramsString.at(name).getVectorParameter(); + break; + } + } + return true; +} + +// Private +// ======= + +bool Parameters::existName(const Parameters::ParamName& name) const +{ + if (existName(name, PARAM_INT) || existName(name, PARAM_BOOL) || existName(name, PARAM_DOUBLE) + || existName(name, PARAM_STRING) || existName(name, PARAM_STRUCT_INT) + || existName(name, PARAM_STRUCT_BOOL) || existName(name, PARAM_STRUCT_DOUBLE) + || existName(name, PARAM_STRUCT_STRING)) { + return true; + } + return false; +} + +bool Parameters::existName(const Parameters::ParamName& name, const wbt::ParameterType& type) const +{ + switch (type) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: + if (m_paramsInt.find(name) == m_paramsInt.end()) { + return false; + } + break; + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: + if (m_paramsBool.find(name) == m_paramsBool.end()) { + return false; + } + break; + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: + if (m_paramsDouble.find(name) == m_paramsDouble.end()) { + return false; + } + break; + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: + if (m_paramsString.find(name) == m_paramsString.end()) { + return false; + } + break; + } + return true; +} + +bool Parameters::existIndex(const Parameters::ParamIndex& index) const +{ + if (m_indexToName.find(index) == m_indexToName.end()) { + return false; + } + + return true; +} + +unsigned Parameters::getNumberOfParameters() const +{ + const unsigned numIntParams = m_paramsInt.size(); + const unsigned numBoolParams = m_paramsBool.size(); + const unsigned numDoubleParams = m_paramsDouble.size(); + const unsigned numStringParams = m_paramsString.size(); + + return numIntParams + numBoolParams + numDoubleParams + numStringParams; +} + +std::vector> Parameters::getIntParameters() const +{ + std::vector> vectorParams; + + for (auto p : m_paramsInt) { + vectorParams.push_back(p.second); + } + + return vectorParams; +} + +std::vector> Parameters::getBoolParameters() const +{ + std::vector> vectorParams; + + for (auto p : m_paramsBool) { + vectorParams.push_back(p.second); + } + + return vectorParams; +} + +std::vector> Parameters::getDoubleParameters() const +{ + std::vector> vectorParams; + + for (auto p : m_paramsDouble) { + vectorParams.push_back(p.second); + } + + return vectorParams; +} + +std::vector> Parameters::getStringParameters() const +{ + std::vector> vectorParams; + + for (auto p : m_paramsString) { + vectorParams.push_back(p.second); + } + + return vectorParams; +} + +wbt::ParameterMetadata Parameters::getParameterMetadata(const ParamName& name) +{ + // TODO: what if name does not exist? ParameterMetadata has no default constructor + + switch (m_nameToType[name]) { + case PARAM_INT: + case PARAM_CELL_INT: + case PARAM_STRUCT_INT: + case PARAM_STRUCT_CELL_INT: + return m_paramsInt.at(name).getMetadata(); + case PARAM_BOOL: + case PARAM_CELL_BOOL: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_CELL_BOOL: + return m_paramsBool.at(name).getMetadata(); + case PARAM_DOUBLE: + case PARAM_CELL_DOUBLE: + case PARAM_STRUCT_DOUBLE: + case PARAM_STRUCT_CELL_DOUBLE: + return m_paramsDouble.at(name).getMetadata(); + case PARAM_STRING: + case PARAM_CELL_STRING: + case PARAM_STRUCT_STRING: + case PARAM_STRUCT_CELL_STRING: + return m_paramsString.at(name).getMetadata(); + } +} + +bool Parameters::containConfigurationData(const wbt::Parameters& parameters) +{ + return parameters.existName("ConfBlockName") && parameters.existName("RobotName") + && parameters.existName("UrdfFile") && parameters.existName("LocalName") + && parameters.existName("ControlledJoints") && parameters.existName("ControlBoardsNames") + && parameters.existName("GravityVector"); +} diff --git a/toolbox/src/base/SimulinkBlockInformation.cpp b/toolbox/src/base/SimulinkBlockInformation.cpp index 3d2eadf3f..293837d97 100644 --- a/toolbox/src/base/SimulinkBlockInformation.cpp +++ b/toolbox/src/base/SimulinkBlockInformation.cpp @@ -1,8 +1,12 @@ #include "SimulinkBlockInformation.h" +#include "Log.h" #include "MxAnyType.h" +#include "Parameters.h" #include "Signal.h" +#include "ToolboxSingleton.h" #include + #include #include @@ -60,6 +64,12 @@ bool SimulinkBlockInformation::getBooleanParameterAtIndex(unsigned parameterInde return MxAnyType(blockParam).asBool(value); } +bool SimulinkBlockInformation::getCellAtIndex(unsigned parameterIndex, AnyCell& vector) const +{ + const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); + return MxAnyType(blockParam).asAnyCell(vector); +} + bool SimulinkBlockInformation::getStructAtIndex(unsigned parameterIndex, AnyStruct& map) const { const mxArray* blockParam = ssGetSFcnParam(simstruct, parameterIndex); @@ -73,6 +83,91 @@ bool SimulinkBlockInformation::getVectorAtIndex(unsigned parameterIndex, return MxAnyType(blockParam).asVectorDouble(vec); } +bool SimulinkBlockInformation::getStringFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + std::string& stringParameter) const +{ + AnyStruct s; + + if (!getStructAtIndex(parameterIndex, s)) { + return false; + } + + if (s.find(fieldName) == s.end()) { + return false; + } + + return s.at(fieldName)->asString(stringParameter); +} + +bool SimulinkBlockInformation::getScalarFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + double& value) const +{ + AnyStruct s; + + if (!getStructAtIndex(parameterIndex, s)) { + return false; + } + + if (s.find(fieldName) == s.end()) { + return false; + } + + return s.at(fieldName)->asDouble(value); +} + +bool SimulinkBlockInformation::getBooleanFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + bool& value) const +{ + AnyStruct s; + + if (!getStructAtIndex(parameterIndex, s)) { + return false; + } + + if (s.find(fieldName) == s.end()) { + return false; + } + + return s.at(fieldName)->asBool(value); +} + +bool SimulinkBlockInformation::getCellFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + AnyCell& cell) const +{ + AnyStruct s; + + if (!getStructAtIndex(parameterIndex, s)) { + return false; + } + + if (s.find(fieldName) == s.end()) { + return false; + } + + return s.at(fieldName)->asAnyCell(cell); +} + +bool SimulinkBlockInformation::getVectorDoubleFieldAtIndex(unsigned parameterIndex, + const std::string& fieldName, + std::vector& vector) const +{ + AnyStruct s; + + if (!getStructAtIndex(parameterIndex, s)) { + return false; + } + + if (s.find(fieldName) == s.end()) { + return false; + } + + return s.at(fieldName)->asVectorDouble(vector); +} + // PORT INFORMATION SETTERS // ======================== @@ -127,13 +222,13 @@ bool SimulinkBlockInformation::setOutputPortMatrixSize(unsigned portNumber, int bool SimulinkBlockInformation::setInputPortType(unsigned portNumber, PortDataType portType) { ssSetInputPortDirectFeedThrough(simstruct, portNumber, 1); - ssSetInputPortDataType(simstruct, portNumber, mapSimulinkToPortType(portType)); + ssSetInputPortDataType(simstruct, portNumber, mapPortTypeToSimulink(portType)); return true; } bool SimulinkBlockInformation::setOutputPortType(unsigned portNumber, PortDataType portType) { - ssSetOutputPortDataType(simstruct, portNumber, mapSimulinkToPortType(portType)); + ssSetOutputPortDataType(simstruct, portNumber, mapPortTypeToSimulink(portType)); return true; } @@ -208,7 +303,7 @@ wbt::Signal SimulinkBlockInformation::getOutputPortSignal(unsigned portNumber, i { // Check if the signal is dynamically sized (which means that the dimension // cannot be read) - bool isDynamicallySized = ssGetOutputPortWidth(simstruct, portNumber) == DYNAMICALLY_SIZED; + bool isDynamicallySized = (ssGetOutputPortWidth(simstruct, portNumber) == DYNAMICALLY_SIZED); // Note that if the signal is DYNAMICALLY_SIZED (-1), portWidth is necessary if (isDynamicallySized && portWidth == -1) { @@ -285,3 +380,288 @@ DTypeId SimulinkBlockInformation::mapPortTypeToSimulink(const PortDataType& data return SS_BOOLEAN; } } + +bool SimulinkBlockInformation::addParameterMetadata(const wbt::ParameterMetadata& paramMD) +{ + // Add the new metadata to the block information + 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 : paramsMetadata) { + + bool ok; + + // TODO Right now the cells are reshaped to a 1 x NumElements by MxAnyType + if (paramMD.rows == DYNAMIC_SIZE) { + return false; + } + + // Handle the case of dynamically sized columns. In this case the metadata passed + // from the Block (containing DYNAMIC_SIZE) is modified with the length of the + // vector that is going to be stored. + const bool hasDynSizeColumns = (paramMD.cols == DYNAMIC_SIZE); + + 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 PARAM_INT: + case PARAM_BOOL: + case PARAM_DOUBLE: { + if (metadataContainsScalarParam(paramMD)) { + double paramValue; + if (!getScalarParameterAtIndex(paramMD.index, paramValue)) { + return false; + } + ok = parameters.storeParameter(paramValue, paramMD); + } + else { + std::vector paramVector; + if (!getVectorAtIndex(paramMD.index, paramVector)) { + return false; + } + if (hasDynSizeColumns) + paramMD.cols = paramVector.size(); + ok = parameters.storeParameter>(paramVector, paramMD); + } + break; + } + case PARAM_STRING: { + if (metadataContainsScalarParam(paramMD)) { + std::string paramValue; + if (!getStringParameterAtIndex(paramMD.index, paramValue)) { + return false; + } + ok = parameters.storeParameter(paramValue, paramMD); + } + else { + // TODO + wbt::Log::getSingleton().error("Char arrays are not yet supported."); + return false; + } + break; + } + // CELL PARAMETERS + // --------------- + case PARAM_CELL_INT: + case PARAM_CELL_BOOL: + case PARAM_CELL_DOUBLE: { + AnyCell cell; + if (!getCellAtIndex(paramMD.index, cell)) { + return false; + } + std::vector paramVector; + for (auto element : cell) { + double value; + if (!element->asDouble(value)) { + return false; + } + paramVector.push_back(value); + } + if (hasDynSizeColumns) + paramMD.cols = paramVector.size(); + ok = parameters.storeParameter>(paramVector, paramMD); + break; + } + case PARAM_CELL_STRING: { + AnyCell cell; + if (!getCellAtIndex(paramMD.index, cell)) { + return false; + } + std::vector paramVector; + for (auto element : cell) { + std::string value; + if (!element->asString(value)) { + return false; + } + paramVector.push_back(value); + } + if (hasDynSizeColumns) + paramMD.cols = paramVector.size(); + ok = parameters.storeParameter(paramVector, paramMD); + break; + } + // STRUCT PARAMETERS + // ----------------- + case PARAM_STRUCT_INT: + case PARAM_STRUCT_BOOL: + case PARAM_STRUCT_DOUBLE: { + if (metadataContainsScalarParam(paramMD)) { + double paramValue; + if (!getScalarFieldAtIndex(paramMD.index, paramMD.name, paramValue)) { + return false; + } + ok = parameters.storeParameter(paramValue, paramMD); + } + else { + std::vector paramVector; + if (!getVectorDoubleFieldAtIndex(paramMD.index, paramMD.name, paramVector)) { + return false; + } + if (hasDynSizeColumns) + paramMD.cols = paramVector.size(); + ok = parameters.storeParameter>(paramVector, paramMD); + } + break; + } + case PARAM_STRUCT_STRING: { + if (metadataContainsScalarParam(paramMD)) { + std::string paramValue; + if (!getStringFieldAtIndex(paramMD.index, paramMD.name, paramValue)) { + return false; + } + ok = parameters.storeParameter(paramValue, paramMD); + } + else { + // TODO + wbt::Log::getSingleton().error("Char arrays are not yet supported."); + return false; + } + break; + } + case PARAM_STRUCT_CELL_INT: + case PARAM_STRUCT_CELL_BOOL: + case PARAM_STRUCT_CELL_DOUBLE: { + AnyCell cell; + std::vector paramVector; + if (!getCellFieldAtIndex(paramMD.index, paramMD.name, cell)) { + return false; + } + for (auto element : cell) { + double value; + if (!element->asDouble(value)) { + return false; + } + paramVector.push_back(value); + } + if (hasDynSizeColumns) + paramMD.cols = paramVector.size(); + ok = parameters.storeParameter(paramVector, paramMD); + break; + } + case PARAM_STRUCT_CELL_STRING: { + AnyCell cell; + std::vector paramVector; + if (!getCellFieldAtIndex(paramMD.index, paramMD.name, cell)) { + return false; + } + for (auto element : cell) { + std::string value; + if (!element->asString(value)) { + return false; + } + paramVector.push_back(value); + } + if (hasDynSizeColumns) + paramMD.cols = paramVector.size(); + ok = parameters.storeParameter>(paramVector, paramMD); + break; + } + } + + if (!ok) + return false; + } + + // This code is shared with the CoderBlockParameter + // + // TODO: is this the most appropriate place for this function? + // + // Check if the parameters object contains all the information for creating a + // Configuration object. + if (Parameters::containConfigurationData(parameters)) { + if (!ToolboxSingleton::sharedInstance().storeConfiguration(parameters)) { + return false; + } + // Save the name of the Configuration block which the processed WBBlock refers to + if (!parameters.getParameter("ConfBlockName", m_confBlockName)) { + // TODO + 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). + paramsMetadata.clear(); + + return true; +} + +// bool SimulinkBlockInformation::fillConfiguration(wbt::Configuration& configuration, const +// wbt::Parameters& parameters) +//{ +// bool ok = true; + +// std::string robotName; +// std::string urdfFile; +// std::string localName; +// std::string confBlockName; +// std::vector gravityVector; +// std::vector controlledJoints; +// std::vector controlBoardsNames; + +// ok = ok && parameters.getParameter("RobotName", robotName); +// ok = ok && parameters.getParameter("UrdfFile", urdfFile); +// ok = ok && parameters.getParameter("LocalName", localName); +// ok = ok && parameters.getParameter("ControlledJoints", controlledJoints); +// ok = ok && parameters.getParameter("ControlBoardsNames", controlBoardsNames); +// ok = ok && parameters.getParameter("GravityVector", gravityVector); +// ok = ok && parameters.getParameter("ConfBlockName", confBlockName); + +// // Populate the Configuration object +// // ================================= + +// configuration = Configuration(confBlockName); +// configuration.setRobotName(robotName); +// configuration.setUrdfFile(urdfFile); +// configuration.setControlledJoints(controlledJoints); +// configuration.setControlBoardsNames(controlBoardsNames); +// configuration.setLocalName(localName); + +// std::array gravityArray; +// for (size_t i = 0; i < 3; ++i) { +// gravityArray[i] = gravityVector[i]; +// } +// configuration.setGravityVector(gravityArray); + +// return ok; +//} + +// bool SimulinkBlockInformation::containConfigurationData(const wbt::Parameters& parameters) +//{ +// return parameters.existName("ConfBlockName") && +// parameters.existName("RobotName") && +// parameters.existName("UrdfFile") && +// parameters.existName("LocalName") && +// parameters.existName("ControlledJoints") && +// parameters.existName("ControlBoardsNames") && +// parameters.existName("GravityVector"); +//} + +std::weak_ptr SimulinkBlockInformation::getRobotInterface() const +{ + // Returns a nullptr if it fails + return ToolboxSingleton::sharedInstance().getRobotInterface(m_confBlockName); +} + +std::weak_ptr SimulinkBlockInformation::getKinDynComputations() const +{ + // Returns a nullptr if it fails + return ToolboxSingleton::sharedInstance().getKinDynComputations(m_confBlockName); +} diff --git a/toolbox/src/base/ToolboxSingleton.cpp b/toolbox/src/base/ToolboxSingleton.cpp index 8dc2b8bec..60ede03c0 100644 --- a/toolbox/src/base/ToolboxSingleton.cpp +++ b/toolbox/src/base/ToolboxSingleton.cpp @@ -1,5 +1,6 @@ #include "ToolboxSingleton.h" #include "Log.h" +#include "Parameters.h" #include "RobotInterface.h" #include @@ -10,6 +11,7 @@ #include using namespace wbt; +bool fillConfiguration(wbt::Configuration& configuration, const wbt::Parameters& parameters); // CONSTRUCTOR / DESTRUCTOR // ======================== @@ -28,10 +30,10 @@ int ToolboxSingleton::numberOfDoFs(const std::string& confKey) return m_interfaces[confKey]->getConfiguration().getNumberOfDoFs(); } -bool ToolboxSingleton::isKeyValid(const std::string& confKey) +bool ToolboxSingleton::isKeyValid(const std::string& confKey) const { if (m_interfaces.find(confKey) != m_interfaces.end()) { - if (m_interfaces[confKey]) + if (m_interfaces.at(confKey)) return true; else return false; @@ -50,40 +52,42 @@ ToolboxSingleton& ToolboxSingleton::sharedInstance() return instance; } -const Configuration& ToolboxSingleton::getConfiguration(const std::string& confKey) +const Configuration& ToolboxSingleton::getConfiguration(const std::string& confKey) const { return getRobotInterface(confKey)->getConfiguration(); } const std::shared_ptr -ToolboxSingleton::getRobotInterface(const std::string& confKey) +ToolboxSingleton::getRobotInterface(const std::string& confKey) const { if (!isKeyValid(confKey)) { return nullptr; } - return m_interfaces[confKey]; + return m_interfaces.at(confKey); } const std::shared_ptr -ToolboxSingleton::getKinDynComputations(const std::string& confKey) +ToolboxSingleton::getKinDynComputations(const std::string& confKey) const { if (!isKeyValid(confKey)) { return nullptr; } - return m_interfaces[confKey]->getKinDynComputations(); + return m_interfaces.at(confKey)->getKinDynComputations(); } // TOOLBOXSINGLETON CONFIGURATION // ============================== -bool ToolboxSingleton::storeConfiguration(const std::string& confKey, const Configuration& config) +bool ToolboxSingleton::storeConfiguration(const Configuration& config) { if (!config.isValid()) { return false; } + const std::string confKey = config.getConfKey(); + // Add the new Configuration object and override an existing key if it already exist. // Note: Simulink doesn't flush memory unless Matlab is closed, and static objects stay in // memory. @@ -109,6 +113,73 @@ bool ToolboxSingleton::storeConfiguration(const std::string& confKey, const Conf return true; } +bool fillConfiguration(wbt::Configuration& configuration, const wbt::Parameters& parameters) +{ + bool ok = true; + + std::string robotName; + std::string urdfFile; + std::string localName; + std::string confBlockName; + std::vector gravityVector; + std::vector controlledJoints; + std::vector controlBoardsNames; + + ok = ok && parameters.getParameter("RobotName", robotName); + ok = ok && parameters.getParameter("UrdfFile", urdfFile); + ok = ok && parameters.getParameter("LocalName", localName); + ok = ok && parameters.getParameter("ControlledJoints", controlledJoints); + ok = ok && parameters.getParameter("ControlBoardsNames", controlBoardsNames); + ok = ok && parameters.getParameter("GravityVector", gravityVector); + ok = ok && parameters.getParameter("ConfBlockName", confBlockName); + + // Populate the Configuration object + // ================================= + + configuration = Configuration(confBlockName); + configuration.setRobotName(robotName); + configuration.setUrdfFile(urdfFile); + configuration.setControlledJoints(controlledJoints); + configuration.setControlBoardsNames(controlBoardsNames); + configuration.setLocalName(localName); + + std::array gravityArray; + for (size_t i = 0; i < 3; ++i) { + gravityArray[i] = gravityVector[i]; + } + configuration.setGravityVector(gravityArray); + + return ok; +} + +bool ToolboxSingleton::storeConfiguration(const wbt::Parameters& parameters) +{ + if (!Parameters::containConfigurationData(parameters)) { + // TODO + return false; + } + + Configuration configuration; + if (!fillConfiguration(configuration, parameters)) { + Log::getSingleton().error("Failed to fill the configuration with input parameters."); + return false; + } + + if (!configuration.isValid()) { + Log::getSingleton().error("Parsed Configuration object is not valid."); + return false; + } + + // Insert the configuration into the Toolbox Singleton + if (!storeConfiguration(configuration)) { + Log::getSingleton().error( + "Failed to store the given configuration in the ToolboxSingleton."); + return false; + } + + return true; +} + void ToolboxSingleton::eraseConfiguration(const std::string& confKey) { m_interfaces.erase(confKey); diff --git a/toolbox/src/base/WBBlock.cpp b/toolbox/src/base/WBBlock.cpp index bc45b6e09..f3fecda02 100644 --- a/toolbox/src/base/WBBlock.cpp +++ b/toolbox/src/base/WBBlock.cpp @@ -1,6 +1,4 @@ #include "WBBlock.h" - -#include "AnyType.h" #include "BlockInformation.h" #include "Configuration.h" #include "Log.h" @@ -8,22 +6,25 @@ #include "Signal.h" #include "ToolboxSingleton.h" -#include "iDynTree/KinDynComputations.h" #include #include #include #include #include #include +#include #include #include using namespace wbt; -const unsigned WBBlock::ConfigurationParameterIndex = 1; // Struct from Simulink -const unsigned WBBlock::ConfBlockNameParameterIndex = 2; // Absolute name of the block containing - // the configuration +const unsigned WBBlock::NumberOfParameters = Block::NumberOfParameters + 2; + +const unsigned PARAM_IDX_BIAS = Block::NumberOfParameters - 1; +const unsigned ConfigurationParameterIndex = PARAM_IDX_BIAS + 1; // Struct from Simulink +const unsigned ConfBlockNameParameterIndex = + PARAM_IDX_BIAS + 2; // Absolute name of the block containing the configuration iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::array& gravity) : m_gravity(gravity.data(), 3) @@ -34,10 +35,26 @@ iDynTreeRobotState::iDynTreeRobotState(const unsigned& dofs, const std::arraygetRobotInterface. +// It might have a reason only to keep here the error message, keeping the output() +// of the block more clean; +std::weak_ptr +WBBlock::getKinDynComputations(BlockInformation* blockInfo) +{ + if (!blockInfo->getRobotInterface().lock()) { + Log::getSingleton().error("Failed to get the RobotInterface object."); + return std::weak_ptr(); + } + + return blockInfo->getRobotInterface().lock()->getKinDynComputations(); +} + bool WBBlock::setRobotState(const wbt::Signal* basePose, const wbt::Signal* jointsPos, const wbt::Signal* baseVelocity, - const wbt::Signal* jointsVelocity) + const wbt::Signal* jointsVelocity, + iDynTree::KinDynComputations* kinDyn) { // SAVE THE ROBOT STATE // ==================== @@ -71,7 +88,7 @@ bool WBBlock::setRobotState(const wbt::Signal* basePose, return false; } // Fill the data - for (auto i = 0; i < jointsPos->getWidth(); ++i) { + for (unsigned i = 0; i < jointsPos->getWidth(); ++i) { robotState.m_jointsPosition.setVal(i, buffer[i]); } } @@ -101,7 +118,7 @@ bool WBBlock::setRobotState(const wbt::Signal* basePose, return false; } // Fill the data - for (auto i = 0; i < jointsVelocity->getWidth(); ++i) { + for (unsigned i = 0; i < jointsVelocity->getWidth(); ++i) { robotState.m_jointsVelocity.setVal(i, buffer[i]); } } @@ -109,18 +126,16 @@ bool WBBlock::setRobotState(const wbt::Signal* basePose, // UPDATE THE IDYNTREE ROBOT STATE WITH NEW DATA // ============================================= - const auto& model = getRobotInterface()->getKinDynComputations(); - - if (!model) { - Log::getSingleton().error("Failed to retrieve the KinDynComputations object."); + if (!kinDyn) { + Log::getSingleton().error("Failed to access the KinDynComputations object."); return false; } - bool ok = model->setRobotState(robotState.m_world_T_base, - robotState.m_jointsPosition, - robotState.m_baseVelocity, - robotState.m_jointsVelocity, - robotState.m_gravity); + bool ok = kinDyn->setRobotState(robotState.m_world_T_base, + robotState.m_jointsPosition, + robotState.m_baseVelocity, + robotState.m_jointsVelocity, + robotState.m_gravity); if (!ok) { Log::getSingleton().error("Failed to set the iDynTree robot state."); @@ -132,138 +147,112 @@ bool WBBlock::setRobotState(const wbt::Signal* basePose, unsigned WBBlock::numberOfParameters() { - return 2; + return WBBlock::NumberOfParameters; } -bool WBBlock::getWBToolboxParameters(Configuration& config, const BlockInformation* blockInfo) +// bool WBBlock::fillConfiguration(Configuration& config) +//{ +// bool ok = true; + +// std::string robotName; +// std::string urdfFile; +// std::string localName; +// std::vector gravityVector; +// std::vector controlledJoints; +// std::vector controlBoardsNames; + +// ok = ok && parameters.getParameter("RobotName", robotName); +// ok = ok && parameters.getParameter("UrdfFile", urdfFile); +// ok = ok && parameters.getParameter("LocalName", localName); +// ok = ok && parameters.getParameter("ControlledJoints", controlledJoints); +// ok = ok && parameters.getParameter("ControlBoardsNames", controlBoardsNames); +// ok = ok && parameters.getParameter("GravityVector", gravityVector); + +// // Populate the Configuration object +// // ================================= + +// config.setRobotName(robotName); +// config.setUrdfFile(urdfFile); +// config.setControlledJoints(controlledJoints); +// config.setControlBoardsNames(controlBoardsNames); +// config.setLocalName(localName); + +// std::array gravityArray; +// for (auto i = 0; i < 3; ++i) { +// gravityArray[i] = gravityVector[i]; +// } +// config.setGravityVector(gravityArray); + +// return ok; +//} + +// bool WBBlock::getConfiguration(BlockInformation* blockInfo, Configuration& configuration) const +//{ +// // Get the RobotInterface containing the Configuration object +// auto robotInterface = blockInfo->getRobotInterface(); + +// if (!robotInterface.lock()) { +// Log::getSingleton().error("RobotInterface has not been correctly initialized."); +// return false; +// } + +// configuration = robotInterface.lock()->getConfiguration(); +// return true; +//} + +// const std::weak_ptr WBBlock::getRobotInterface(BlockInformation* blockInfo) const +//{ +// return blockInfo->getRobotInterface(); +//} + +bool WBBlock::parseParameters(BlockInformation* blockInfo) { - // Infos - // ===== - // - // For what concern initialization, the callback order is: - // - // -> configureSizeAndPorts() - // -> initialize() - // -> initializeInitialConditions() - // - // Despite class objects after the configureSizeAndPorts() are destroyed and - // reallocated before the initialize(), the ToolboxSingleton remains alive and - // can store from this stage the configuration created by reading the parameters. - // - - // Gather the parameters from the Mask of the Simulink Block - // ========================================================= - - // Get the struct containing the parameters - AnyStruct s; - if (!blockInfo->getStructAtIndex(ConfigurationParameterIndex, s)) { - Log::getSingleton().error("Failed to retrieve the struct with parameters."); + if (!Block::parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse Block parameters."); return false; } - // Check the existence of all the fields - try { - s.at("RobotName"); - s.at("UrdfFile"); - s.at("ControlledJoints"); - s.at("ControlBoardsNames"); - s.at("LocalName"); - s.at("GravityVector"); - } - catch (const std::out_of_range& e) { - Log::getSingleton().error("Cannot retrieve one or more parameter from config struct."); - return false; - } + ParameterMetadata fieldRobotNameMedatata( + PARAM_STRUCT_STRING, ConfigurationParameterIndex, 1, 1, "RobotName"); - // RobotName - std::string robotName; - if (!s["RobotName"]->asString(robotName)) { - Log::getSingleton().error("Cannot retrieve string from RobotName parameter."); - return false; - } + ParameterMetadata fieldUrdfFileMedatata( + PARAM_STRUCT_STRING, ConfigurationParameterIndex, 1, 1, "UrdfFile"); - // UrdfFile - std::string urdfFile; - if (!s["UrdfFile"]->asString(urdfFile)) { - Log::getSingleton().error("Cannot retrieve string from UrdfFile parameter."); - return false; - } + ParameterMetadata fieldControlledJointsMedatata( + PARAM_STRUCT_CELL_STRING, ConfigurationParameterIndex, 1, DYNAMIC_SIZE, "ControlledJoints"); - // ControlledJoints - AnyCell cellCJ; - if (!s["ControlledJoints"]->asAnyCell(cellCJ)) { - Log::getSingleton().error("Cannot retrieve ControlledJoints parameter."); - return false; - } + ParameterMetadata fieldControlBoardsMedatata(PARAM_STRUCT_CELL_STRING, + ConfigurationParameterIndex, + 1, + DYNAMIC_SIZE, + "ControlBoardsNames"); - std::vector controlledJoints; - for (auto cell : cellCJ) { - std::string joint; - if (!cell->asString(joint)) { - Log::getSingleton().error("Failed to convert ControlledJoints from cell to strings."); - return false; - } - controlledJoints.push_back(joint); - } + ParameterMetadata fieldLocalNameMedatata( + PARAM_STRUCT_STRING, ConfigurationParameterIndex, 1, 1, "LocalName"); - // ControlBoardsNames - AnyCell cellCBN; - if (!s["ControlBoardsNames"]->asAnyCell(cellCBN)) { - Log::getSingleton().error("Cannot retrieve ControlBoardsNames parameter."); - return false; - } + ParameterMetadata fieldGravityVectorMedatata( + PARAM_STRUCT_DOUBLE, ConfigurationParameterIndex, 1, 3, "GravityVector"); - std::vector controlBoardsNames; - for (auto cell : cellCBN) { - std::string joint; - if (!cell->asString(joint)) { - Log::getSingleton().error("Failed to convert ControlBoardsNames from cell to string."); - return false; - } - controlBoardsNames.push_back(joint); - } + ParameterMetadata confBlockNameMedatata( + PARAM_STRING, ConfBlockNameParameterIndex, 1, 1, "ConfBlockName"); - // LocalName - std::string localName; - if (!s["LocalName"]->asString(localName)) { - Log::getSingleton().error("Cannot retrieve string from LocalName parameter."); - return false; - } + // Add the struct into the block information + bool ok = true; + ok = ok && blockInfo->addParameterMetadata(fieldRobotNameMedatata); + ok = ok && blockInfo->addParameterMetadata(fieldUrdfFileMedatata); + ok = ok && blockInfo->addParameterMetadata(fieldLocalNameMedatata); + ok = ok && blockInfo->addParameterMetadata(fieldControlledJointsMedatata); + ok = ok && blockInfo->addParameterMetadata(fieldControlBoardsMedatata); + ok = ok && blockInfo->addParameterMetadata(fieldGravityVectorMedatata); + // + ok = ok && blockInfo->addParameterMetadata(confBlockNameMedatata); - std::vector gravityVector; - if (!s["GravityVector"]->asVectorDouble(gravityVector)) { - Log::getSingleton().error("Cannot retrieve vector from GravityVector parameter."); + if (!ok) { + Log::getSingleton().error("Failed to store parameters metadata."); return false; } - std::array gravityArray; - for (auto i = 0; i < 3; ++i) { - gravityArray[i] = gravityVector[i]; - } - // Create the ToolboxConfig object - // =============================== - - // Populate a Property object that stores the input parameters - config.setRobotName(robotName); - config.setUrdfFile(urdfFile); - config.setControlledJoints(controlledJoints); - config.setControlBoardsNames(controlBoardsNames); - config.setLocalName(localName); - config.setGravityVector(gravityArray); - - return true; -} - -const Configuration& WBBlock::getConfiguration() -{ - ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); - return interface.getConfiguration(confKey); -} - -const std::shared_ptr WBBlock::getRobotInterface() -{ - ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); - return interface.getRobotInterface(confKey); + return blockInfo->parseParameters(parameters); } bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) @@ -277,38 +266,66 @@ bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) // -------------------------------------------------------------- // Initialize the ToolboxSingleton - ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + // blockInfo-> + // ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); // Get the absolute name of the block from which the configuration has been retrieved. // This will be the key of the map containing all the configurations of the ToolboxSingleton. - if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { - Log::getSingleton().error("Failed to retrieve the struct with parameters."); + // if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { + // Log::getSingleton().error("Failed to retrieve the struct with parameters."); + // return false; + // } + + // Parse the parameters + // if (!WBBlock::parseParameters(blockInfo)) { + // Log::getSingleton().error("Failed to parse parameters."); + // return false; + // } + + // // Get the name of the Configuration block which this block refers to + // if (!parameters.getParameter("ConfBlockName", confKey)) { + // Log::getSingleton().error("Failed to get the name of the config block parameter."); + // return false; + // } + + // // Initialize the configuration block with the unique identifier of the conf block name + // Configuration config(confKey); + + // // Populate the Configuration object with the parsed parameters + // if (!fillConfiguration(config)) { + // return false; + // } + + // // Check if the configuration is valid + // if (!config.isValid()) { + // Log::getSingleton().error("The provided configuration is not valid."); + // return false; + // } + + // // Store the configuration inside the ToolboxSingleton + // if (!interface.storeConfiguration(confKey, config)) { + // Log::getSingleton().error("Failed to configure ToolboxSingleton."); + // return false; + // } + + // Parse the parameters + if (!WBBlock::parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); return false; } - // Initialize the configuration block with the unique identifier of the conf block name - Configuration config(confKey); - // Populate the configuration object with data from the Simulink's struct parameter - if (!getWBToolboxParameters(config, blockInfo)) { - return false; - } - - // Check if the configuration is valid - if (!config.isValid()) { - Log::getSingleton().error("The provided configuration is not valid."); - return false; - } + // Configuration conf; + // if getConfiguration(blockInfo, conf); - // Store the configuration inside the ToolboxSingleton - if (!interface.storeConfiguration(confKey, config)) { - Log::getSingleton().error("Failed to configure ToolboxSingleton."); + // Get the RobotInterface containing the Configuration object + auto robotInterface = blockInfo->getRobotInterface(); + if (!robotInterface.lock()) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); return false; } - // Check if the DoFs are positive (and if the config has been stored successfully) - const Configuration& configFromSingleton = getConfiguration(); - - if (configFromSingleton.getNumberOfDoFs() < 1) { + // Check if the DoFs are positive + if (robotInterface.lock()->getConfiguration().getNumberOfDoFs() < 1) { Log::getSingleton().error("Failed to configure WBBlock. Read 0 DoFs."); return false; } @@ -316,32 +333,47 @@ bool WBBlock::configureSizeAndPorts(BlockInformation* blockInfo) return true; } -bool WBBlock::initialize(const BlockInformation* blockInfo) +bool WBBlock::initialize(BlockInformation* blockInfo) { // CONFIGURE the ToolboxSingleton // ============================== - ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); + // ToolboxSingleton& interface = ToolboxSingleton::sharedInstance(); // Gather again confKey // Note: despite data for blocks is not persistent between configureSizeAndPorts() // and initialize() (e.g. confKey), the singleton is not destroyed. // This means that the stored configurations are still there. - if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { - Log::getSingleton().error("Failed to retrieve the struct with parameters."); + // if (!blockInfo->getStringParameterAtIndex(ConfBlockNameParameterIndex, confKey)) { + // Log::getSingleton().error("Failed to retrieve the struct with parameters."); + // return false; + // } + // Parse the parameters + if (!WBBlock::parseParameters(blockInfo)) { + Log::getSingleton().error("Failed to parse parameters."); return false; } + // Get the name of the Configuration block which this block refers to + // if (!parameters.getParameter("ConfBlockName", confKey)) { + // Log::getSingleton().error("Failed to get the name of the config block parameter."); + // return false; + // } // Check if the key is valid - if (!interface.isKeyValid(confKey)) { - Log::getSingleton().error( - "Failed to retrieve the configuration object from the singleton."); + // if (!interface.isKeyValid(confKey)) { + // Log::getSingleton().error("Failed to retrieve the configuration object from the + // singleton."); return false; + // } + + auto robotInterface = blockInfo->getRobotInterface(); + if (!robotInterface.lock()) { + Log::getSingleton().error("RobotInterface has not been correctly initialized."); return false; } - // Initialize the iDynTreeRobotState struct - const unsigned& dofs = interface.numberOfDoFs(confKey); - robotState = iDynTreeRobotState(dofs, getConfiguration().getGravityVector()); + const unsigned& dofs = robotInterface.lock()->getConfiguration().getNumberOfDoFs(); + robotState = + iDynTreeRobotState(dofs, robotInterface.lock()->getConfiguration().getGravityVector()); return true; } diff --git a/toolbox/src/base/WBToolbox.cpp b/toolbox/src/base/WBToolbox.cpp new file mode 100644 index 000000000..d6d42d1b1 --- /dev/null +++ b/toolbox/src/base/WBToolbox.cpp @@ -0,0 +1,696 @@ +#define S_FUNCTION_LEVEL 2 +#define S_FUNCTION_NAME WBToolbox + +#include "Block.h" +#include "Log.h" +#include "SimulinkBlockInformation.h" +#include "toolbox.h" + +#include +#include +#include + +const bool ForwardLogsToStdErr = true; + +static void catchLogMessages(bool status, SimStruct* S, std::string prefix) +{ + // Initialize static buffers + const unsigned bufferLen = 1024; + static char errorBuffer[bufferLen]; + static char warningBuffer[bufferLen]; + + // Notify warnings + if (!wbt::Log::getSingleton().getWarnings().empty()) { + // Get the singleton + wbt::Log& log = wbt::Log::getSingleton(); + + // Handle the prefix + std::string warningMsg; + if (!prefix.empty()) { + log.setPrefix(prefix); + warningMsg = log.getWarnings(); + log.resetPrefix(); + } + else { + warningMsg = log.getWarnings(); + } + + // Trim the message if needed + if (warningMsg.length() >= bufferLen) { + warningMsg = warningMsg.substr(0, bufferLen - 1); + } + + // Forward to Simulink + sprintf(warningBuffer, "%s", warningMsg.c_str()); + ssWarning(S, warningBuffer); + if (ForwardLogsToStdErr) { + fprintf(stderr, "%s", errorBuffer); + } + log.clearWarnings(); + } + + // Notify errors + if (!status) { + // Get the singleton + wbt::Log& log = wbt::Log::getSingleton(); + + // Handle the prefix + std::string errorMsg; + if (!prefix.empty()) { + log.setPrefix(prefix); + errorMsg = log.getErrors(); + log.resetPrefix(); + } + else { + errorMsg = log.getErrors(); + } + + // Trim the message if needed + if (errorMsg.length() >= bufferLen) { + errorMsg = errorMsg.substr(0, bufferLen - 1); + } + + // Forward to Simulink + sprintf(errorBuffer, "%s", errorMsg.c_str()); + ssSetErrorStatus(S, errorBuffer); + + if (ForwardLogsToStdErr) { + fprintf(stderr, "%s", errorBuffer); + } + log.clearErrors(); + return; + } +} + +// 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) +{ + // TODO: for now accept the proposed size. + // If we want to change the behaviour we have to implement some callbacks + ssSetInputPortDimensionInfo(S, port, dimsInfo); +} + +#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO +static void mdlSetOutputPortDimensionInfo(SimStruct* S, int_T port, const DimsInfo_T* dimsInfo) +{ + // TODO: for now accept the proposed size. + // If we want to change the behaviour we have to implement some callbacks + ssSetOutputPortDimensionInfo(S, port, dimsInfo); +} + +// 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) { + wbt::Log::getSingleton().error("The block type parameter must be specified"); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); + std::string className(classNameStr); + mxFree(classNameStr); + wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); + + // Notify errors + if (!block) { + wbt::Log::getSingleton().error("Could not create an object of type " + className); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + + // We cannot save data in PWork during the initializeSizes phase. + + // Two PWorks: + // 0: pointer to the class + // 1: empty in Simulink, pointer to CoderBlockInformation in Simulink Coder + 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 { + wbt::Log::getSingleton().error("Number of parameters different from those defined"); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } +#endif + + wbt::SimulinkBlockInformation blockInfo(S); + bool ok = block->configureSizeAndPorts(&blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + + 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 (auto additionalOption : additionalOptions) { + double option; + if (blockInfo.optionFromKey(additionalOption, option)) { + options |= static_cast(option); + } + } + ssSetOptions(S, options); + + delete block; +} + +// 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 for allocating the object + char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); + std::string className(classNameStr); + mxFree(classNameStr); + + // Allocate the object and store its pointer in the PWork + wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); + 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) { + wbt::Log::getSingleton().error( + "Failed to create objects before storing them in the PWork."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + + // Call the initialize() method + bool ok = block->initialize(blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); +} + +#define MDL_UPDATE +#if defined(MDL_UPDATE) && defined(MATLAB_MEX_FILE) +static void mdlUpdate(SimStruct* S, int_T tid) +{ + UNUSED_ARG(tid); + if (ssGetNumPWork(S) != 2) { + wbt::Log::getSingleton().error("PWork should contain two elements."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + 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) { + wbt::Log::getSingleton().error("Failed to get pointers from the PWork vector."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + + // Call the updateDiscreteState() method + bool ok = block->updateDiscreteState(blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); +} +#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) { + wbt::Log::getSingleton().error("PWork should contain two elements."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + 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) { + wbt::Log::getSingleton().error("Failed to get pointers from the PWork vector."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + + // Call the initializeInitialConditions() method + bool ok = block->initializeInitialConditions(blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); +} +#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) { + wbt::Log::getSingleton().error("PWork should contain two elements."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + 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) { + wbt::Log::getSingleton().error("Failed to get pointers from the PWork vector."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + + // Call the output() method + bool ok = block->output(blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); +} + +static void mdlTerminate(SimStruct* S) +{ + // This function is called during the initialization phase, when the PWorks are not yet + // allocated. Only the number of elements is known at this stage. + // This check will pass if called after the initialize(), that stores data in the PWorks. + if (!ssGetPWork(S)) { + return; + } + + if (ssGetNumPWork(S) != 2) { + wbt::Log::getSingleton().error("PWork should contain two elements."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + 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) { + wbt::Log::getSingleton().error("Failed to get pointers from the PWork vector."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + + // Call the terminate() method + bool ok = block->terminate(blockInfo); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + + // Delete the resources allocated in the PWork vector; + delete block; + delete blockInfo; + + // Clean the PWork vector + ssSetPWorkValue(S, 0, nullptr); + ssSetPWorkValue(S, 1, nullptr); +} + +#if defined(MATLAB_MEX_FILE) +#define MDL_RTW + +template +static std::vector toRTWNumericVector(const std::vector& vectorInput) +{ + std::vector output; + output.reserve(vectorInput.size()); + + output.assign(vectorInput.begin(), vectorInput.end()); + return output; +} + +static std::string toRTWStringVector(const std::vector& stringInput) +{ + std::string output; + + for (auto 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::PARAM_INT: + return {"PARAM_INT", "int"}; + case wbt::PARAM_BOOL: + return {"PARAM_BOOL", "bool"}; + case wbt::PARAM_DOUBLE: + return {"PARAM_DOUBLE", "double"}; + case wbt::PARAM_STRING: + return {"PARAM_STRING", "std::string"}; + case wbt::PARAM_CELL_INT: + return {"PARAM_CELL_INT", "int"}; + case wbt::PARAM_CELL_BOOL: + return {"PARAM_CELL_BOOL", "bool"}; + case wbt::PARAM_CELL_DOUBLE: + return {"PARAM_CELL_DOUBLE", "double"}; + case wbt::PARAM_CELL_STRING: + return {"PARAM_CELL_STRING", "std::string"}; + case wbt::PARAM_STRUCT_INT: + return {"PARAM_STRUCT_INT", "int"}; + case wbt::PARAM_STRUCT_BOOL: + return {"PARAM_STRUCT_BOOL", "bool"}; + case wbt::PARAM_STRUCT_DOUBLE: + return {"PARAM_STRUCT_DOUBLE", "double"}; + case wbt::PARAM_STRUCT_STRING: + return {"PARAM_STRUCT_STRING", "std::string"}; + case wbt::PARAM_STRUCT_CELL_INT: + return {"PARAM_STRUCT_CELL_INT", "int"}; + case wbt::PARAM_STRUCT_CELL_BOOL: + return {"PARAM_STRUCT_CELL_BOOL", "bool"}; + case wbt::PARAM_STRUCT_CELL_DOUBLE: + return {"PARAM_STRUCT_CELL_DOUBLE", "double"}; + case wbt::PARAM_STRUCT_CELL_STRING: + return {"PARAM_STRUCT_CELL_STRING", "std::string"}; + } +} + +template +static bool writeParameterToRTW(const wbt::Parameter param, SimStruct* S) +{ + 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.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); + } +} + +// static std::string generateUniquePWorkName(SimStruct *S) +//{ +// std::string stringID(ssGetPath(S)); + +// auto toRemove = [](const char& c) { +// const std::string forbiddenChars { " /-_\\" }; +// return std::string::npos != forbiddenChars.find(c); +// }; + +// stringID.erase(remove_if(stringID.begin(), stringID.end(), toRemove), stringID.end()); + +// return "PWork" + stringID; +//} + +static 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); + } + + 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) { + wbt::Log::getSingleton().error("Unable to get the class from the PWork vector."); + catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); + return; + } + + // Get the parameters from the block + ok = block->getParameters(params); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + if (!ok) + return; + + // Use parameters metadata to populate the rtw file used by the coder + ok = writeRTW(S, params); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + if (!ok) + return; + + // Store the PWork vector in the rtw file. + // TODO ok = ssWriteRTWWorkVect(S, "PWork", 1, generateUniquePWorkName(S).c_str() , + // ssGetNumPWork(S)); + ok = ssWriteRTWWorkVect(S, "PWork", 1, "blockPWork", ssGetNumPWork(S)); + catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); + if (!ok) + return; + } +} +#endif + +// Required S-function trailer +#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ +#include "simulink.c" /* MEX-file interface mechanism */ +#else +#include "cg_sfun.h" /* Code generation registration function */ +#endif diff --git a/toolbox/src/base/factory.cpp b/toolbox/src/base/factory.cpp index 16fff95ff..8c7f67dcc 100644 --- a/toolbox/src/base/factory.cpp +++ b/toolbox/src/base/factory.cpp @@ -6,23 +6,19 @@ Block* Block::instantiateBlockWithClassName(std::string blockClassName) { Block* block = nullptr; - if (blockClassName == YarpRead::ClassName) { - block = new YarpRead(); - } - else if (blockClassName == YarpWrite::ClassName) { - block = new YarpWrite(); - } - else if (blockClassName == ModelPartitioner::ClassName) { - block = new ModelPartitioner(); - } - else if (blockClassName == YarpClock::ClassName) { + // if (blockClassName == YarpRead::ClassName) { + // block = new YarpRead(); + // } else if (blockClassName == YarpWrite::ClassName) { + // block = new YarpWrite(); + // } else if (blockClassName == ModelPartitioner::ClassName) { + // block = new ModelPartitioner(); + // } else if (blockClassName == YarpClock::ClassName) { + if (blockClassName == YarpClock::ClassName) { block = new YarpClock(); - } - else if (blockClassName == MassMatrix::ClassName) { - block = new MassMatrix(); - } - else if (blockClassName == ForwardKinematics::ClassName) { - block = new ForwardKinematics(); + // } else if (blockClassName == MassMatrix::ClassName) { + // block = new MassMatrix(); + // } else if (blockClassName == ForwardKinematics::ClassName) { + // block = new ForwardKinematics(); } else if (blockClassName == SetReferences::ClassName) { block = new SetReferences(); @@ -32,31 +28,27 @@ Block* Block::instantiateBlockWithClassName(std::string blockClassName) } else if (blockClassName == SimulatorSynchronizer::ClassName) { block = new SimulatorSynchronizer(); - } - else if (blockClassName == Jacobian::ClassName) { - block = new Jacobian(); + // } else if (blockClassName == Jacobian::ClassName) { + // block = new Jacobian(); } else if (blockClassName == GetMeasurement::ClassName) { block = new GetMeasurement(); - } - else if (blockClassName == InverseDynamics::ClassName) { - block = new InverseDynamics(); + // } else if (blockClassName == InverseDynamics::ClassName) { + // block = new InverseDynamics(); } else if (blockClassName == DotJNu::ClassName) { block = new DotJNu(); - } - else if (blockClassName == GetLimits::ClassName) { - block = new GetLimits(); + // } else if (blockClassName == GetLimits::ClassName) { + // block = new GetLimits(); } else if (blockClassName == CentroidalMomentum::ClassName) { block = new CentroidalMomentum(); - } - else if (blockClassName == SetLowLevelPID::ClassName) { - block = new SetLowLevelPID(); - } + // } else if (blockClassName == SetLowLevelPID::ClassName) { + // block = new SetLowLevelPID(); + // } #ifdef WBT_USES_ICUB - else if (blockClassName == MinimumJerkTrajectoryGenerator::ClassName) { - block = new MinimumJerkTrajectoryGenerator(); + // else if (blockClassName == MinimumJerkTrajectoryGenerator::ClassName) { + // block = new MinimumJerkTrajectoryGenerator(); } else if (blockClassName == DiscreteFilter::ClassName) { block = new wbt::DiscreteFilter(); diff --git a/toolbox/src/base/toolbox.cpp b/toolbox/src/base/toolbox.cpp deleted file mode 100644 index d6bf0934e..000000000 --- a/toolbox/src/base/toolbox.cpp +++ /dev/null @@ -1,335 +0,0 @@ -/* - * Copyright (C) 2013-2015 Robotics, Brain and Cognitive Sciences - Istituto Italiano di Tecnologia - * Author: Jorhabib Eljaik Gomez, Francesco Romano - * email: jorhabib.eljaik@iit.it, francesco.romano@iit.it - * - * The development of this software was supported by the FP7 EU project - * CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics (b)) - * http://www.codyco.eu - * - * Permission is granted to copy, distribute, and/or modify this program - * under the terms of the GNU General Public License, version 2 or any - * later version published by the Free Software Foundation. - * - * This program is distributed in the hope that it will be useful, but - * WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General - * Public License for more details - */ - -#define S_FUNCTION_LEVEL 2 -#define S_FUNCTION_NAME WBToolbox - -// Need to include simstruc.h for the definition of the SimStruct and -// its associated macro definitions. - -#include "toolbox.h" -#include "Block.h" -#include "Log.h" -#include "SimulinkBlockInformation.h" - -#include -#include -#include - -static void catchLogMessages(bool status, SimStruct* S, std::string prefix) -{ - // Initialize static buffers - const unsigned bufferLen = 1024; - static char errorBuffer[bufferLen]; - static char warningBuffer[bufferLen]; - - // Notify warnings - if (!wbt::Log::getSingleton().getWarnings().empty()) { - // Get the singleton - wbt::Log& log = wbt::Log::getSingleton(); - - // Handle the prefix - std::string warningMsg; - if (!prefix.empty()) { - log.setPrefix(prefix); - warningMsg = log.getWarnings(); - log.resetPrefix(); - } - else { - warningMsg = log.getWarnings(); - } - - // Trim the message if needed - if (warningMsg.length() >= bufferLen) { - warningMsg = warningMsg.substr(0, bufferLen - 1); - } - - // Forward to Simulink - sprintf(warningBuffer, "%s", warningMsg.c_str()); - ssWarning(S, warningBuffer); - log.clearWarnings(); - } - - // Notify errors - if (!status) { - // Get the singleton - wbt::Log& log = wbt::Log::getSingleton(); - - // Handle the prefix - std::string errorMsg; - if (!prefix.empty()) { - log.setPrefix(prefix); - errorMsg = log.getErrors(); - log.resetPrefix(); - } - else { - errorMsg = log.getErrors(); - } - - // Trim the message if needed - if (errorMsg.length() >= bufferLen) { - errorMsg = errorMsg.substr(0, bufferLen - 1); - } - - // Forward to Simulink - sprintf(errorBuffer, "%s", errorMsg.c_str()); - ssSetErrorStatus(S, errorBuffer); - log.clearErrors(); - return; - } -} - -// 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) -{ - // TODO: for now accept the proposed size. - // If we want to change the behaviour we have to implement some callbacks - ssSetInputPortDimensionInfo(S, port, dimsInfo); -} - -#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO -static void mdlSetOutputPortDimensionInfo(SimStruct* S, int_T port, const DimsInfo_T* dimsInfo) -{ - // TODO: for now accept the proposed size. - // If we want to change the behaviour we have to implement some callbacks - ssSetOutputPortDimensionInfo(S, port, dimsInfo); -} - -// 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) { - wbt::Log::getSingleton().error("The block type parameter must be specified"); - catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); - return; - } - char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); - std::string className(classNameStr); - mxFree(classNameStr); - wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); - - // We cannot save data in PWork during the initializeSizes phase - ssSetNumPWork(S, 1); - - // Notify errors - if (!block) { - wbt::Log::getSingleton().error("Could not create an object of type " + className); - catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); - return; - } - - ssSetNumSFcnParams(S, 1 + block->numberOfParameters()); - ssSetSFcnParamTunable(S, 0, false); - for (unsigned i = 1; i < ssGetNumSFcnParams(S); ++i) { - bool tunable = false; - block->parameterAtIndexIsTunable(i - 1, tunable); - ssSetSFcnParamTunable(S, i, tunable); - } - -#if defined(MATLAB_MEX_FILE) - if (ssGetNumSFcnParams(S) == ssGetSFcnParamsCount(S)) { - mdlCheckParameters(S); - if (ssGetErrorStatus(S)) { - return; - } - } - else { - wbt::Log::getSingleton().error("Number of parameters different from those defined"); - catchLogMessages(false, S, "\n[" + std::string(__func__) + "]"); - return; - } -#endif - - wbt::SimulinkBlockInformation blockInfo(S); - bool ok = block->configureSizeAndPorts(&blockInfo); - catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); - - for (auto i = 0; i < ssGetNumInputPorts(S); ++i) { - // Set explicitly the inputs port to be SS_NOT_REUSABLE_AND_GLOBAL (which actually - // is already the default value). Since the toolbox supports contiguous input signals, - // this option should not be changed. - ssSetInputPortOptimOpts(S, i, SS_NOT_REUSABLE_AND_GLOBAL); - // Set input signals to be allocated in a contiguous memory storage - ssSetInputPortRequiredContiguous(S, i, true); - } - - ssSetNumSampleTimes(S, 1); - - 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 (auto additionalOption : additionalOptions) { - double option; - if (blockInfo.optionFromKey(additionalOption, option)) { - options |= static_cast(option); - } - } - ssSetOptions(S, options); - - delete block; -} - -// 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) -{ - char* classNameStr = mxArrayToString(ssGetSFcnParam(S, 0)); - std::string className(classNameStr); - mxFree(classNameStr); - wbt::Block* block = wbt::Block::instantiateBlockWithClassName(className); - ssSetPWorkValue(S, 0, block); - - wbt::SimulinkBlockInformation blockInfo(S); - bool ok = false; - if (block) { - ok = block->initialize(&blockInfo); - } - catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); -} - -#define MDL_UPDATE -#if defined(MDL_UPDATE) && defined(MATLAB_MEX_FILE) -static void mdlUpdate(SimStruct* S, int_T tid) -{ - UNUSED_ARG(tid); - if (ssGetNumPWork(S) > 0) { - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - - wbt::SimulinkBlockInformation blockInfo(S); - bool ok = false; - if (block) { - ok = block->updateDiscreteState(&blockInfo); - } - catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); - } -} -#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) > 0) { - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - - wbt::SimulinkBlockInformation blockInfo(S); - bool ok = false; - if (block) { - ok = block->initializeInitialConditions(&blockInfo); - } - catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); - } -} -#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) > 0) { - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - - wbt::SimulinkBlockInformation blockInfo(S); - bool ok = false; - if (block) { - ok = block->output(&blockInfo); - } - catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); - } -} - -static void mdlTerminate(SimStruct* S) -{ - if (ssGetNumPWork(S) > 0 && ssGetPWork(S)) { - wbt::Block* block = static_cast(ssGetPWorkValue(S, 0)); - - wbt::SimulinkBlockInformation blockInfo(S); - bool ok = false; - if (block) { - ok = block->terminate(&blockInfo); - } - catchLogMessages(ok, S, "\n[" + std::string(__func__) + "]"); - - if (ok) { - delete block; - ssSetPWorkValue(S, 0, NULL); - } - } -} - -// Required S-function trailer -#ifdef MATLAB_MEX_FILE /* Is this file being compiled as a MEX-file? */ -#include "simulink.c" /* MEX-file interface mechanism */ -#else -#include "cg_sfun.h" /* Code generation registration function */ -#endif