From 82c5c4296200839d6aee69fdc0e857beb8c90ead Mon Sep 17 00:00:00 2001 From: Diego Ferigo Date: Fri, 6 Apr 2018 14:12:01 +0200 Subject: [PATCH] Improved documentation - Big improvements of doxygen documentation - Minor edits in mkdocs website - Fixes https://github.com/robotology/wb-toolbox/issues/100 --- README.md | 2 +- doc/doxygen/index.md | 2 +- doc/mkdocs/data/about.md | 2 +- doc/mkdocs/data/install.md | 1 + toolbox/include/CentroidalMomentum.h | 3 + toolbox/include/DiscreteFilter.h | 20 ++ toolbox/include/DotJNu.h | 12 + toolbox/include/ForwardKinematics.h | 12 + toolbox/include/GetLimits.h | 12 + toolbox/include/GetMeasurement.h | 12 + toolbox/include/InverseDynamics.h | 3 + toolbox/include/Jacobian.h | 12 + toolbox/include/MassMatrix.h | 3 + .../include/MinimumJerkTrajectoryGenerator.h | 19 +- toolbox/include/ModelPartitioner.h | 11 + toolbox/include/RealTimeSynchronizer.h | 12 + toolbox/include/SetLowLevelPID.h | 15 + toolbox/include/SetReferences.h | 13 + toolbox/include/SimulatorSynchronizer.h | 14 + toolbox/include/YarpClock.h | 3 + toolbox/include/YarpRead.h | 16 ++ toolbox/include/YarpWrite.h | 12 + toolbox/include/base/Block.h | 262 +++++++++++++----- toolbox/include/base/BlockInformation.h | 196 +++++++++++-- toolbox/include/base/Configuration.h | 111 +++++--- toolbox/include/base/Log.h | 46 ++- toolbox/include/base/Parameter.h | 22 ++ toolbox/include/base/Parameters.h | 122 +++++++- toolbox/include/base/RobotInterface.h | 143 +++++----- toolbox/include/base/Signal.h | 177 +++++++++++- toolbox/include/base/ToolboxSingleton.h | 15 +- toolbox/include/base/WBBlock.h | 64 +++-- 32 files changed, 1130 insertions(+), 239 deletions(-) diff --git a/README.md b/README.md index 5f97c3077..1b763b254 100644 --- a/README.md +++ b/README.md @@ -45,4 +45,4 @@ year={2017}, The development of WB-Toolbox is supported by: - FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics) -- H2020 EU projects An.Dy. (No. 731540 H2020-ICT-2016-1). +- H2020 EU projects AnDy (No. 731540 H2020-ICT-2016-1) diff --git a/doc/doxygen/index.md b/doc/doxygen/index.md index 37dbbbd88..e77019b31 100644 --- a/doc/doxygen/index.md +++ b/doc/doxygen/index.md @@ -35,4 +35,4 @@ Bibtex citation: The development of WB-Toolbox is supported by: - FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics) -- H2020 EU projects An.Dy. (No. 731540 H2020-ICT-2016-1). +- H2020 EU projects AnDy (No. 731540 H2020-ICT-2016-1) diff --git a/doc/mkdocs/data/about.md b/doc/mkdocs/data/about.md index e7a500191..003879562 100644 --- a/doc/mkdocs/data/about.md +++ b/doc/mkdocs/data/about.md @@ -24,4 +24,4 @@ In alphabetical order: The development of `WB-Toolbox` is supported by: - FP7 EU projects CoDyCo (No. 600716 ICT 2011.2.1 Cognitive Systems and Robotics) -- H2020 EU projects An.Dy (No. 731540 H2020-ICT-2016-1). +- H2020 EU projects AnDy (No. 731540 H2020-ICT-2016-1) diff --git a/doc/mkdocs/data/install.md b/doc/mkdocs/data/install.md index aa2594322..714991829 100644 --- a/doc/mkdocs/data/install.md +++ b/doc/mkdocs/data/install.md @@ -9,6 +9,7 @@ - Matlab 7.1+ and Simulink: tested with Matlab `R2017b`, `R2016b` - [`YARP`](https://github.com/robotology/yarp) compiled as shared library (default behavior) - [`iDynTree`](https://github.com/robotology/idyntree) +- [`YCM`](https://github.com/robotology/ycm) - Supported Operating Systems: Linux, macOS, Windows ## Optional requirements diff --git a/toolbox/include/CentroidalMomentum.h b/toolbox/include/CentroidalMomentum.h index 0cc4ebe07..cc485d96f 100644 --- a/toolbox/include/CentroidalMomentum.h +++ b/toolbox/include/CentroidalMomentum.h @@ -20,6 +20,9 @@ namespace iDynTree { class SpatialMomentum; } +/** + * @brief The wbt::CentroidalMomentum class + */ class wbt::CentroidalMomentum : public wbt::WBBlock { private: diff --git a/toolbox/include/DiscreteFilter.h b/toolbox/include/DiscreteFilter.h index 119144d8a..d16274e5d 100644 --- a/toolbox/include/DiscreteFilter.h +++ b/toolbox/include/DiscreteFilter.h @@ -31,6 +31,26 @@ namespace yarp { } } // namespace yarp +/** + * @brief The wbt::DiscreteFilter class + * + * @section Parameters + * + * In addition to @ref block_parameters, wbt::DiscreteFilter requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "Fc" | + * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "Ts" | + * | ::STRUCT_INT | 0 + Block::NumberOfParameters | 1 | 1 | "MedianOrder" | + * | ::STRUCT_STRING | 0 + Block::NumberOfParameters | 1 | 1 | "FilterType" | + * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | ::DynamicSize | "NumCoeffs" | + * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | ::DynamicSize | "DenCoeffs" | + * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "InitStatus" | + * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "y0" | + * | ::STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "u0" | + * + */ class wbt::DiscreteFilter : public wbt::Block { private: diff --git a/toolbox/include/DotJNu.h b/toolbox/include/DotJNu.h index 7d3d4c4f1..e674c73c7 100644 --- a/toolbox/include/DotJNu.h +++ b/toolbox/include/DotJNu.h @@ -18,6 +18,18 @@ namespace wbt { class DotJNu; } +/** + * @brief The wbt::DotJNu class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::DotJNu requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "Frame" | + * + */ class wbt::DotJNu : public wbt::WBBlock { private: diff --git a/toolbox/include/ForwardKinematics.h b/toolbox/include/ForwardKinematics.h index 043a76d7c..c1e958824 100644 --- a/toolbox/include/ForwardKinematics.h +++ b/toolbox/include/ForwardKinematics.h @@ -17,6 +17,18 @@ namespace wbt { class ForwardKinematics; } +/** + * @brief The wbt::ForwardKinematics class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::ForwardKinematics requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "Frame" | + * + */ class wbt::ForwardKinematics : public wbt::WBBlock { private: diff --git a/toolbox/include/GetLimits.h b/toolbox/include/GetLimits.h index b9813cae6..ab2a3f82a 100644 --- a/toolbox/include/GetLimits.h +++ b/toolbox/include/GetLimits.h @@ -28,6 +28,18 @@ struct wbt::Limit {} }; +/** + * @brief The wbt::GetLimits class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::GetLimits requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::DOUBLE | 0 + WBBlock::NumberOfParameters | 1 | 1 | "LimitType" | + * + */ class wbt::GetLimits : public wbt::WBBlock { private: diff --git a/toolbox/include/GetMeasurement.h b/toolbox/include/GetMeasurement.h index b0f9ac6f5..c92ce9482 100644 --- a/toolbox/include/GetMeasurement.h +++ b/toolbox/include/GetMeasurement.h @@ -22,6 +22,18 @@ namespace wbt { }; } // namespace wbt +/** + * @brief The wbt::GetMeasurement class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::GetMeasurement requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "MeasuredType" | + * + */ class wbt::GetMeasurement : public wbt::WBBlock { private: diff --git a/toolbox/include/InverseDynamics.h b/toolbox/include/InverseDynamics.h index 6214ac4ba..152d8e48f 100644 --- a/toolbox/include/InverseDynamics.h +++ b/toolbox/include/InverseDynamics.h @@ -22,6 +22,9 @@ namespace iDynTree { class FreeFloatingGeneralizedTorques; } // namespace iDynTree +/** + * @brief The wbt::InverseDynamics class + */ class wbt::InverseDynamics : public wbt::WBBlock { private: diff --git a/toolbox/include/Jacobian.h b/toolbox/include/Jacobian.h index 72042b8fd..5c1cb3f71 100644 --- a/toolbox/include/Jacobian.h +++ b/toolbox/include/Jacobian.h @@ -21,6 +21,18 @@ namespace iDynTree { class MatrixDynSize; } +/** + * @brief The wbt::Jacobian class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::Jacobian requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "Frame" | + * + */ class wbt::Jacobian : public wbt::WBBlock { private: diff --git a/toolbox/include/MassMatrix.h b/toolbox/include/MassMatrix.h index c23f89af0..3ca5cba0d 100644 --- a/toolbox/include/MassMatrix.h +++ b/toolbox/include/MassMatrix.h @@ -20,6 +20,9 @@ namespace iDynTree { class MatrixDynSize; } +/** + * @brief The wbt::MassMatrix class + */ class wbt::MassMatrix : public wbt::WBBlock { private: diff --git a/toolbox/include/MinimumJerkTrajectoryGenerator.h b/toolbox/include/MinimumJerkTrajectoryGenerator.h index 38b65a56f..87cb5e518 100644 --- a/toolbox/include/MinimumJerkTrajectoryGenerator.h +++ b/toolbox/include/MinimumJerkTrajectoryGenerator.h @@ -10,7 +10,6 @@ #define WBT_MINJERKTRAJGENERATOR_H #include "Block.h" - #include #include @@ -25,6 +24,24 @@ namespace iCub { } } // namespace iCub +/** + * @brief The wbt::MinimumJerkTrajectoryGenerator class + * + * @section Parameters + * + * In addition to @ref block_parameters, wbt::MinimumJerkTrajectoryGenerator requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "SampleTime" | + * | ::DOUBLE | 1 + Block::NumberOfParameters | 1 | 1 | "SettlingTime" | + * | ::BOOL | 2 + Block::NumberOfParameters | 1 | 1 | "ComputeFirstDerivative" | + * | ::BOOL | 3 + Block::NumberOfParameters | 1 | 1 | "ComputeSecondDerivative" | + * | ::BOOL | 4 + Block::NumberOfParameters | 1 | 1 | "ReadInitialValue" | + * | ::BOOL | 5 + Block::NumberOfParameters | 1 | 1 | "ReadExternalSettlingTime" | + * | ::BOOL | 6 + Block::NumberOfParameters | 1 | 1 | "ResetOnSettlingTimeChange" | + * + */ class wbt::MinimumJerkTrajectoryGenerator : public wbt::Block { private: diff --git a/toolbox/include/ModelPartitioner.h b/toolbox/include/ModelPartitioner.h index 3a1e9dfb5..3956dca5f 100644 --- a/toolbox/include/ModelPartitioner.h +++ b/toolbox/include/ModelPartitioner.h @@ -18,6 +18,17 @@ namespace wbt { class ModelPartitioner; } +/** + * @brief The wbt::ModelPartitioner class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::ModelPartitioner requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::BOOL | 0 + WBBlock::NumberOfParameters | 1 | 1 | "VectorToControlBoards" | + */ class wbt::ModelPartitioner : public wbt::WBBlock { private: diff --git a/toolbox/include/RealTimeSynchronizer.h b/toolbox/include/RealTimeSynchronizer.h index e48c9dc6f..b746c2fa4 100644 --- a/toolbox/include/RealTimeSynchronizer.h +++ b/toolbox/include/RealTimeSynchronizer.h @@ -15,6 +15,18 @@ namespace wbt { class RealTimeSynchronizer; } +/** + * @brief The wbt::RealTimeSynchronizer class + * + * @section Parameters + * + * In addition to @ref block_parameters, wbt::RealTimeSynchronizer requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::DOUBLE | 0 + Block::NumberOfParameters | 1 | 1 | "Period" | + * + */ class wbt::RealTimeSynchronizer : public wbt::Block { private: diff --git a/toolbox/include/SetLowLevelPID.h b/toolbox/include/SetLowLevelPID.h index 8c7ddc0b8..53e3d79a0 100644 --- a/toolbox/include/SetLowLevelPID.h +++ b/toolbox/include/SetLowLevelPID.h @@ -25,6 +25,21 @@ namespace yarp { } } // namespace yarp +/** + * @brief The wbt::SetLowLevelPID class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::SetLowLevelPID requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRUCT_CELL_DOUBLE | 0 + WBBlock::NumberOfParameters | 1 | 1 | "P" | + * | ::STRUCT_CELL_DOUBLE | 0 + WBBlock::NumberOfParameters | 1 | 1 | "I" | + * | ::STRUCT_CELL_DOUBLE | 0 + WBBlock::NumberOfParameters | 1 | 1 | "D" | + * | ::STRING | 1 + WBBlock::NumberOfParameters | 1 | 1 | "ControlType" | + * + */ class wbt::SetLowLevelPID : public wbt::WBBlock { private: diff --git a/toolbox/include/SetReferences.h b/toolbox/include/SetReferences.h index 313e318e4..aba79b929 100644 --- a/toolbox/include/SetReferences.h +++ b/toolbox/include/SetReferences.h @@ -16,6 +16,19 @@ namespace wbt { class SetReferences; } +/** + * @brief The wbt::SetReferences class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::SetReferences requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRING | 0 + WBBlock::NumberOfParameters | 1 | 1 | "CtrlType" | + * | ::DOUBLE | 0 + WBBlock::NumberOfParameters | 1 | 1 | "RefSpeed" | + * + */ class wbt::SetReferences : public wbt::WBBlock { private: diff --git a/toolbox/include/SimulatorSynchronizer.h b/toolbox/include/SimulatorSynchronizer.h index e3b2f0a43..c4bfa995b 100644 --- a/toolbox/include/SimulatorSynchronizer.h +++ b/toolbox/include/SimulatorSynchronizer.h @@ -16,6 +16,20 @@ namespace wbt { class SimulatorSynchronizer; } +/** + * @brief The wbt::SimulatorSynchronizer class + * + * @section Parameters + * + * In addition to @ref wbblock_parameters, wbt::SetReferences requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::DOUBLE | 0 + WBBlock::NumberOfParameters | 1 | 1 | "Period" | + * | ::STRING | 1 + WBBlock::NumberOfParameters | 1 | 1 | "RpcPort" | + * | ::STRING | 2 + WBBlock::NumberOfParameters | 1 | 1 | "GazeboClockPort" | + * + */ class wbt::SimulatorSynchronizer : public wbt::Block { private: diff --git a/toolbox/include/YarpClock.h b/toolbox/include/YarpClock.h index 9b66e3904..3a61191ce 100644 --- a/toolbox/include/YarpClock.h +++ b/toolbox/include/YarpClock.h @@ -15,6 +15,9 @@ namespace wbt { class YarpClock; } +/** + * @brief The wbt::YarpClock class + */ class wbt::YarpClock : public wbt::Block { public: diff --git a/toolbox/include/YarpRead.h b/toolbox/include/YarpRead.h index ab2b7037f..b2a5a0a1f 100644 --- a/toolbox/include/YarpRead.h +++ b/toolbox/include/YarpRead.h @@ -26,6 +26,22 @@ namespace yarp { } } // namespace yarp +/** + * @brief The wbt::YarpRead class + * + * In addition to @ref block_parameters, wbt::YarpRead requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRING | 0 + Block::NumberOfParameters | 1 | 1 | "PortName" | + * | ::INT | 1 + Block::NumberOfParameters | 1 | 1 | "SignalSize" | + * | ::BOOL | 2 + Block::NumberOfParameters | 1 | 1 | "WaitData" | + * | ::BOOL | 3 + Block::NumberOfParameters | 1 | 1 | "ReadTimestamp" | + * | ::BOOL | 4 + Block::NumberOfParameters | 1 | 1 | "Autoconnect" | + * | ::DOUBLE | 5 + Block::NumberOfParameters | 1 | 1 | "Timeout" | + * | ::BOOL | 6 + Block::NumberOfParameters | 1 | 1 | "ErrorOnMissingPort" | + * + */ class wbt::YarpRead : public wbt::Block { private: diff --git a/toolbox/include/YarpWrite.h b/toolbox/include/YarpWrite.h index 9f75b81af..3d898c6b3 100644 --- a/toolbox/include/YarpWrite.h +++ b/toolbox/include/YarpWrite.h @@ -24,6 +24,18 @@ namespace yarp { } } // namespace yarp +/** + * @brief The wbt::YarpWrite class + * + * In addition to @ref block_parameters, wbt::YarpWrite requires: + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::STRING | 0 + Block::NumberOfParameters | 1 | 1 | "PortName" | + * | ::BOOL | 1 + Block::NumberOfParameters | 1 | 1 | "Autoconnect" | + * | ::BOOL | 2 + Block::NumberOfParameters | 1 | 1 | "ErrorOnMissingPort" | + * + */ class wbt::YarpWrite : public wbt::Block { private: diff --git a/toolbox/include/base/Block.h b/toolbox/include/base/Block.h index bde62ba48..705b18280 100644 --- a/toolbox/include/base/Block.h +++ b/toolbox/include/base/Block.h @@ -20,173 +20,287 @@ namespace wbt { } // namespace wbt /** - * Basic abstract class for all the blocks. - * This class (the whole toolbox in reality) assumes the block represent - * an instantaneous system (i.e. not a dynamic system). + * @brief Basic abstract class for wrapping generic algorithms * - * You can create a new block by deriving this class and implementing at least - * all the pure virtual methods. + * ### Rationale * - * @Note: if you need to implement a block which uses the WBI, you should derive - * from WBIBlock as it already provides some facilities. + * This class is aimed to wrap generic algorithms and it represents the most basic component of + * `WB-Toolbox`. + * + * The entire execution of an generic algorithm can be split in the following major steps: + * + * - Initialization: implemented with Block::configureSizeAndPorts, Block::initialize + * - Execution: implemented with Block::output + * - Termination: implemented with Block::terminate + * + * Considering that the main aim of this toolbox at its beginning was its integration with Simulink, + * it contains other methods for exposing the algorithm as a Simulink block. + * + * ### Main concept + * + * A generic application is the composition of many algorithms sharing and processing each other + * data. Every algorithm is represented by a wbt::Block, and the data shared with other blocks is + * carried by wbt::Signal. + * + * The block is generally unaware of the data that it will process, and it is only interested in + * knowing how many input / output signals are connected, their size and data type. Blocks have the + * concept of _ports_, which are the connections between external wbt::Signal and the block itself. + * + * @remark A signal can be plugged to more than one port. + * + * This kind of information is set by a wbt::BlockInformation object, and it can be specific to the + * framework where the algorithm runs (e.g. standalone C++ code, Simulink, etc). + * + * Beyond this, an algorithm often needs parameters. This class provides functionalities to gather + * them in a wbt::Parameters object. + * + * ### Other information + * + * You can create a new block by deriving this class and implementing at least all the pure virtual + * methods. + * + * @note This class and the entire toolbox assume that algorithms are represented as instantaneous + * systems, there is no default storage capability between different sampling times. However, + * blocks can wrap generic classes which can retain data, outsourcing data persistence outside + * the framework given by `WB-Toolbox`. + * \par + * @note Despite some of the methods inside this class looks Simulink-dependent, objects of this + * class are completely generic. In fact, it only provides algorithm callbacks, and the + * setting of input / output data is demanded to the wbt::BlockInformation interface. For what + * concerns Simulink, a wbt::SimulinkBlockInformation implementation is provided. + * \par + * + * @see wbt::BlockInformation Class for providing information about signals and parameters. + * @see wbt::WBBlock Specialization of wbt::Block which provides useful resources for developing + * whole-body blocks. + * + * @section block_parameters Block Parameters + * + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :--: | :--: | ---- | + * | PARAM_STRING | 0 | 1 | 1 | "className" | + * + * @see wbt::ParameterMetadata::ParameterMetadata for the data types of these variables. */ class wbt::Block { protected: + /// Container for block's parameter. You can get this member using Block::getParameters Parameters m_parameters; public: /** - * Create and returns a new Block object of the specified class. + * @brief Create and returns a new Block object of the specified class * - * If the class does not exist returns NULL - * @param blockClassName the derived class name to be instantiated + * If the class does not exist returns `nullptr`. * - * @return the newly created Block object or NULL. + * @param blockClassName The derived class name to be instantiated. + * @return The newly created Block object or `nullptr`. */ static wbt::Block* instantiateBlockWithClassName(const std::string& blockClassName); /** - * Destructor - * + * @brief Destructor */ virtual ~Block() = default; + /** + * Static variable matching Block::numberOfParameters. It might be useful to define parametric + * constants for parameter indices in child blocks. + * + * @see WBBlock::NumberOfParameters + */ static const unsigned NumberOfParameters; /** - * Returns the number of configuration parameters needed by this block + * @brief Returns the number of configuration parameters needed by this block * - * @return the number of parameters + * @return The number of parameters. */ virtual unsigned numberOfParameters(); /** - * Returns vector of additional block options + * @brief Returns the vector of additional block options + * + * Implement this method if you want to store in the Block additional options that can be parsed + * later from BlockInformation::optionFromKey. * - * @return vector containing a list of options + * @return A vector containing a list of options. */ virtual std::vector additionalBlockOptions(); /** - * Returns the number of discrete states of the block. + * @brief Returns the number of discrete states of the block * - * The base implementation returns 0, i.e. no discrete states - * @note if you return a number > 0, you should implement the - * updateDiscreteState function - * @return the number of discrete states + * The base implementation returns 0, i.e. no discrete states. + * + * @note If you return a number > 0, you should implement the Block::updateDiscreteState + * function. + * @return The number of discrete states. */ virtual unsigned numberOfDiscreteStates(); /** - * Returns the number of continuous states of the block. + * @brief Returns the number of continuous states of the block + * + * The base implementation returns 0, i.e. no continuous states. * - * The base implementation returns 0, i.e. no continuous states - * @note if you return a number > 0, you should implement the - * stateDerivative function - * @return the number of continuous states + * @note If you return a number > 0, you should implement the Block::stateDerivative function. + * @return The number of continuous states. */ virtual unsigned numberOfContinuousStates(); /** - * Called to update the internal discrete state + * @brief Update the internal discrete state * - * i.e. x[i+1] = f(x[i]) - * @param S the SimStruct structure - * @return true for success, false otherwise + * i.e. `x[i+1] = f(x[i])` + * + * @param blockInfo A BlockInformation object. + * @return True for success, false otherwise. */ virtual bool updateDiscreteState(const BlockInformation* blockInfo); /** - * Not called for now + * @brief Update the internal continuous state * - * @param S the SimStruct structure - * @return true for success, false otherwise + * @param blockInfo The SimStruct structure. + * @return True for success, false otherwise. */ virtual bool stateDerivative(const BlockInformation* blockInfo); /** - * Specify if the parameter at the specified index is tunable + * @brief Specify if the parameter at the specified index is tunable + * + * Tunable means that it can be changed during the simulation. Usually parameters are defined + * before the beginning of the simulation and they stay constant for all its duration. + * + * @note For the time being tunable parameters are not used in this toolbox. * - * Tunable means that it can be changed during the simulation. - * @Note by default it specifies false for every parameter - * @param [in]index index of the parameter - * @param [out]tunable true if the parameter is tunable. False otherwise + * @param[in] index Index of the parameter. + * @param[out] tunable True if the parameter is tunable, false otherwise. */ virtual void parameterAtIndexIsTunable(unsigned index, bool& tunable); + /** + * @brief Parse the parameters stored into the BlockInformation object + * + * Implement this method to create the metadata of the parameters your block needs (using + * wbt::ParameterMetadata), store them into the blockInfo object with + * BlockInformation::addParameterMetadata, and parse them using + * BlockInformation::parseParameters. + * + * An example of the implementation is the following: + * + * ```cpp + * bool Jacobian::parseParameters(BlockInformation* blockInfo) + * { + * ParameterMetadata frameMetadata(PARAM_STRING, PARAM_IDX_FRAME, 1, 1, "frame"); + * bool ok = blockInfo->addParameterMetadata(frameMetadata); + * + * if (!ok) { + * wbtError << "Failed to store parameters metadata."; + * return false; + * } + + * return blockInfo->parseParameters(m_parameters); + * } + * ``` + * + * @param blockInfo The pointer to a BlockInformation object. + * @return True for success, false otherwise. + * @see BlockInformation::addParameterMetadata, BlockInformation::parseParameters + */ virtual bool parseParameters(BlockInformation* blockInfo); /** - * Configure the input and output ports + * @brief Gather all the stored parameters * - * This method is called at configuration time (i.e. during mdlInitializeSizes) - * In this method you have to configure the number of input and output ports, - * their size and configuration. - * @Note: you should not save any object in this method because it will not persist - * @param S simulink structure + * After the parameters have been successfully stored and parsed using the + * Block::parseParameter, you can gather them using this method. * - * @return true for success, false otherwise + * @param[out] params A wbt::Parameters object containing block's parameters. + * @return bool True for success, false otherwise. + */ + bool getParameters(wbt::Parameters& params) const; + + /** + * @brief Configure the input and output ports + * + * Implement this method to set information about number and size of input and output signals. + * The terminology `port` comes as Simulink inheritage, and it marks the connection of a signal + * (which resides in some buffer in the program memory) to the block's input or output. + * + * These information can be used later (e.g. in the Block::initialize and Block::output) for + * preallocating resources and accessing data knowing its size in advance. + * + * @note If the size is not known at this stage (Signal::DynamicSize), at latest it should be + * set in the Block::initialize step + * @warning Do not allocate any data in this stage! Object are destroyed afterwards and created + * again before the Block::initialize step. Every allocated memory and stored values + * will be deleted. Only information stored in `blockInfo` will persist. + * + * @param blockInfo The pointer to a BlockInformation object. + * @return True if the block was configured successfully, false otherwise. + * @see BlockInformation::setNumberOfInputPorts, BlockInformation::setInputPortVectorSize */ virtual bool configureSizeAndPorts(BlockInformation* blockInfo); /** * Never called. * - * @param S simulink structure - * - * @return true for success, false otherwise + * @param blockInfo The pointer to a BlockInformation object. + * @return True for success, false otherwise. */ virtual bool checkParameters(const BlockInformation* blockInfo); /** - * Initialize the object for the simulation + * @brief Initialize the block * - * This method is called at model startup (i.e. during mdlStart) - * @Note: you can save and initialize your object in this method - * @param S simulink structure + * Implement this method to initialize and allocate the resources the algorithm needs during its + * execution. * - * @return true for success, false otherwise + * @param blockInfo The pointer to a BlockInformation object. + * @return True for success, false otherwise. */ virtual bool initialize(BlockInformation* blockInfo); /** - * Called to initialize the initial conditions + * @brief Initialize block's initial conditions + * + * Implement this method to specify block's initial conditions. Its execution will happen after + * Block::initialize and before the first call of Block::output. The default is an empty + * implementation. * - * The default implementation do nothing. - * @note this function is also called on a reset event - * @note if you need to perform initialization only once, than implement initialize - * @param S simulink structure + * @note this function is also called on a reset event. In Simulink, an example is when the + * block resides in an enabled subsystem. * - * @return true for success, false otherwise + * @param blockInfo The pointer to a BlockInformation object. + * @return True for success, false otherwise. */ virtual bool initializeInitialConditions(const BlockInformation* blockInfo); /** - * Perform model cleanup. + * @brief Cleanup block resources * - * This method is called at model termination (i.e. during mdlTerminate). - * You should release all the resources you requested during the initialize method - * @param S simulink structure + * This method is called during the termination of the execution. Implement this method to + * deallocate all the memory requested during the previous steps or to perform other terminating + * operations. * - * @return true for success, false otherwise + * @param blockInfo The pointer to a BlockInformation object. + * @return True for success, false otherwise. */ virtual bool terminate(const BlockInformation* blockInfo) = 0; /** - * Compute the output of the block + * @brief Compute the output of the block * - * This method is called at every iteration of the model (i.e. during mdlOutput) - * @param S simulink structure - * @param [out]error output error object that can be filled in case of error. + * This method is called at every iteration of the model. Implement here a single step of the + * algorithm. * - * @return true for success, false otherwise + * @param blockInfo The pointer to a BlockInformation object. + * @return True for success, false otherwise. */ virtual bool output(const BlockInformation* blockInfo) = 0; - - virtual bool getParameters(wbt::Parameters& params) const; - -public: }; #endif // WBT_BLOCK_H diff --git a/toolbox/include/base/BlockInformation.h b/toolbox/include/base/BlockInformation.h index ffe4457c9..8bd499266 100644 --- a/toolbox/include/base/BlockInformation.h +++ b/toolbox/include/base/BlockInformation.h @@ -22,6 +22,7 @@ namespace wbt { class Configuration; class RobotInterface; enum class DataType; + // List of possible key for defining block options: extern const std::string BlockOptionPrioritizeOrder; } // namespace wbt @@ -29,6 +30,26 @@ namespace iDynTree { class KinDynComputations; } +/** + * @brief Abstract class for storing generic Block properties + * + * BlockInformation provides an interface for handling implementation-specific properties such as + * input / output number, size and type, number of parameters, ... + * + * A wbt::Block needs to know on what kind of data it operates, and retrieving this information is + * often specific on the framework on top of which block runs. In order to allow using the same + * Block class from different frameworks (e.g. Simulink, C++, etc), different implementation of this + * interface can provide a transparent translation on functionalities. + * + * As an example, take the BlockInformation::parseParameters. In Simulink parameters are read from + * block's masks and Matlab provides a library for reading them. The SimulinkBlockInformation + * implementation will be linked against that library. However, if you want to call the same Block + * class (which is just a wrapper of an algorithm) from a pure C++ main, parameters are read e.g. + * from an xml file. In this case, BlockInformation::parseParameters will parse the xml and fill the + * wbt::Parameters argument. + * + * @see wbt::Block, wbt::Parameters, wbt::Signal + */ class wbt::BlockInformation { public: @@ -41,59 +62,194 @@ class wbt::BlockInformation BlockInformation() = default; virtual ~BlockInformation() = default; + // ===================== // BLOCK OPTIONS METHODS // ===================== /** - * Convert a block option from its Toolbox identifier to a specific implementation + * @brief Convert a block option from its string identifier to a specific implementation * - * @param [in] key identifier of the block option - * @param [out] option implementation-specific block option - * @return true if the option has been converted. False otherwise + * @param[in] key Identifier of the block option. + * @param[out] option Implementation-specific block option. + * @return True if the option has been converted, false otherwise. */ virtual bool optionFromKey(const std::string& key, double& option) const; + // ================== // PARAMETERS METHODS // ================== + /** + * @brief Parse the wbt::Block's parameters + * + * This method allows defining how to gather block's parameters from a specific implementation. + * + * @param[out] parameters A container filled with the parsed parameters. + * @return True for success, false otherwise. + */ virtual bool parseParameters(wbt::Parameters& /*parameters*/) { return true; } + + /** + * @brief Add a parameter metadata + * + * In order to parse parameters with BlockInformation::parseParameters, adding in advance their + * metadata can strongly simplify the entire process. + * + * @param paramMD The metadata to add. + * @return True for success, false otherwise. + */ virtual bool addParameterMetadata(const wbt::ParameterMetadata& /*paramMD*/) { return true; } + // ======================== // PORT INFORMATION SETTERS // ======================== + /** + * @brief Set the number of block's inputs + * + * @param numberOfPorts Number of input ports. + * @return True for success, false otherwise. + */ virtual bool setNumberOfInputPorts(const unsigned& numberOfPorts) = 0; + + /** + * @brief Set the number of block's outputs + * + * @param numberOfPorts Number of output ports. + * @return True for success, false otherwise. + */ virtual bool setNumberOfOutputPorts(const unsigned& numberOfPorts) = 0; - virtual bool setInputPortVectorSize(const SignalIndex& idx, const VectorSize& size) = 0; - virtual bool setInputPortMatrixSize(const SignalIndex& idx, const MatrixSize& size) = 0; - virtual bool setOutputPortVectorSize(const SignalIndex& idx, const VectorSize& size) = 0; - virtual bool setOutputPortMatrixSize(const SignalIndex& idx, const MatrixSize& size) = 0; /** - * Set data type for the specified input port + * @brief Set the size of a 1D input port + * + * @param idx The index of the port. + * @param size The size of the port. + * @return True for success, false otherwise. + */ + virtual bool setInputPortVectorSize(const PortIndex& idx, const VectorSize& size) = 0; + + /** + * @brief Set the size of a 1D output port * - * @param portNumber number of input port - * @param portType data type + * @param idx The index of the port. + * @param size The size of the port. + * @return True for success, false otherwise. + */ + virtual bool setOutputPortVectorSize(const PortIndex& idx, const VectorSize& size) = 0; + + /** + * @brief Set the size of a 2D input port * - * @return true if succeded, false otherwise + * @param idx The index of the port. + * @param size The size of the port. + * @return True for success, false otherwise. */ - virtual bool setInputPortType(const SignalIndex& idx, const DataType& type) = 0; - virtual bool setOutputPortType(const SignalIndex& idx, const DataType& type) = 0; + virtual bool setInputPortMatrixSize(const PortIndex& idx, const MatrixSize& size) = 0; + /** + * @brief Set the size of a 2D output port + * + * @param idx The index of the port. + * @param size The size of the port. + * @return True for success, false otherwise. + */ + virtual bool setOutputPortMatrixSize(const PortIndex& idx, const MatrixSize& size) = 0; + + /** + * @brief Set the data type of an input port + * + * @param idx The index of the port. + * @param type The type of the port. + * @return True for success, false otherwise. + */ + virtual bool setInputPortType(const PortIndex& idx, const DataType& type) = 0; + + /** + * @brief Set the data type of an output port + * + * @param idx The index of the port. + * @param type The type of the port. + * @return True for success, false otherwise. + */ + virtual bool setOutputPortType(const PortIndex& idx, const DataType& type) = 0; + + // ======================== // PORT INFORMATION GETTERS // ======================== - virtual unsigned getInputPortWidth(const SignalIndex& idx) const = 0; - virtual unsigned getOutputPortWidth(const SignalIndex& idx) const = 0; - virtual MatrixSize getInputPortMatrixSize(const SignalIndex& port) const = 0; - virtual MatrixSize getOutputPortMatrixSize(const SignalIndex& port) const = 0; + /** + * @brief Get the size of a 1D input port + * + * @param idx The index of the port. + * @return The size of the port. + */ + virtual unsigned getInputPortWidth(const PortIndex& idx) const = 0; + + /** + * @brief Get the size of a 1D output port + * + * @param idx The index of the port. + * @return The size of the port. + */ + virtual unsigned getOutputPortWidth(const PortIndex& idx) const = 0; + + /** + * @brief Get the size of a 2D input port + * + * @param idx The index of the port. + * @return The size of the port. + */ + virtual MatrixSize getInputPortMatrixSize(const PortIndex& idx) const = 0; + + /** + * @brief Get the size of a 2D output port + * + * @param idx The index of the port. + * @return The size of the port. + */ + virtual MatrixSize getOutputPortMatrixSize(const PortIndex& idx) const = 0; + + // ============= + // BLOCK SIGNALS + // ============= - virtual wbt::Signal getInputPortSignal(const SignalIndex& idx, + /** + * @brief Get the signal connected to a 1D input port + * + * @param idx The index of the port. + * @param size The size of the signal. + * @return The signal connected to the input port for success, an invalid signal otherwise. + * @see Signal::isValid + */ + virtual wbt::Signal getInputPortSignal(const PortIndex& idx, const VectorSize& size = -1) const = 0; - virtual wbt::Signal getOutputPortSignal(const SignalIndex& idx, + + /** + * @brief Get the signal connected to a 1D output port + * + * @param idx The index of the port. + * @param size The size of the signal. + * @return The signal connected to the output port for success, an invalid signal otherwise. + * @see Signal::isValid + */ + virtual wbt::Signal getOutputPortSignal(const PortIndex& idx, const VectorSize& size = -1) const = 0; + // ========================== + // EXTERNAL LIBRARIES METHODS + // ========================== + + /** + * @brief Get the wbt::RobotInterface object associated to the Block + * @return The pointer to the wbt::RobotInterface object + */ virtual std::weak_ptr getRobotInterface() const = 0; + + /** + * @brief Get the iDynTree::KinDynComputations object associated to the Block + * @return The pointer to the iDynTree::KinDynComputations object + */ virtual std::weak_ptr getKinDynComputations() const = 0; }; diff --git a/toolbox/include/base/Configuration.h b/toolbox/include/base/Configuration.h index 12ed2c8c5..9b0114274 100644 --- a/toolbox/include/base/Configuration.h +++ b/toolbox/include/base/Configuration.h @@ -18,42 +18,48 @@ namespace wbt { } /** - * \class Configuration Configuration.h + * @brief Store the configuration for whole-body blocks * - * This class stores in a C++ object the content of the WBToolboxConfig Matlab class. + * This class stores the content of the `WBToolbox.Configuration` Matlab class and its related + * _configuration_ block. This in principle is Simulink specific, but nothing prevents using it from + * C++ for instantiating a wbt::RobotInterface object. * - * @see WBToolboxConfig Matlab Class - * @see RobotInterface + * Its usage is specific for using yarp::dev::RemoteControlBoardRemapper and + * iDynTree::KinDynComputations objects contained in the wbt::RobotInterface class. + * + * @see wbt::RobotInterface, wbt::WBBlock, wbt::ToolboxSingleton, WBToolbox.Configuration Matlab + * Class */ class wbt::Configuration { private: - 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 (unique identifier) std::string m_robotName; ///< Name of the robot std::string m_urdfFile; ///< Name of the file containing the urdf model std::string m_localName; ///< Prefix appended to the opened ports std::vector m_controlledJoints; ///< Subset of controlled joints std::vector m_controlBoardsNames; ///< Names of the used ControlBoard names std::array m_gravityVector; ///< The gravity vector - size_t m_dofs; //< DoFs extracted my m_controlBoardsNames vector + size_t m_dofs; ///< DoFs extracted from Configuration::m_controlBoardsNames vector public: Configuration() = default; Configuration(const std::string& confKey); ~Configuration() = default; + // =========== // SET METHODS // =========== /** - * Initialize the Configuration object + * @brief Initialize the object * - * @param robotName Name of the robot - * @param urdfFile Name of the file containing the urdf model - * @param controlledJoints Subset of controlled joints - * @param controlBoardsNames Names of the used ControlBoard names - * @param localName Prefix appended to the opened ports - * @param gravityVector The gravity vector + * @param robotName Name of the robot. + * @param urdfFile Name of the file containing the urdf model. + * @param controlledJoints Subset of controlled joints. + * @param controlBoardsNames Names of the used ControlBoard names. + * @param localName Prefix appended to the opened ports. + * @param gravityVector The gravity vector. */ void setParameters(const std::string& robotName, const std::string& urdfFile, @@ -63,118 +69,133 @@ class wbt::Configuration const std::array& gravityVector); /** - * Set the name of the robot + * @brief Set the name of the robot * - * @param robotName Name of the robot + * @param robotName Name of the robot. */ void setRobotName(const std::string& robotName); /** - * Set the name of the file containing the urdf model + * @brief Set the name of the file containing the urdf model * - * @param urdfFile Name of the file containing the urdf model + * @param urdfFile Name of the file containing the urdf model. */ void setUrdfFile(const std::string& urdfFile); /** - * Set the subset of controlled joints - * @param controlledJoints Subset of controlled joints + * @brief Set the subset of controlled joints + * + * @param controlledJoints Subset of controlled joints. */ void setControlledJoints(const std::vector& controlledJoints); /** - * Set the names of the used ControlBoard names - * @param controlBoardsNames Names of the used ControlBoard names + * @brief Set the names of the used ControlBoard names + * + * @param controlBoardsNames Names of the used ControlBoard names. */ void setControlBoardsNames(const std::vector& controlBoardsNames); /** - * Set the prefix appended to the opened ports - * @param localName Prefix appended to the opened ports + * @brief Set the prefix appended to the opened ports + * + * @param localName Prefix appended to the opened ports. */ void setLocalName(const std::string& localName); /** - * Set the gravity vector - * @param gravityVector The gravity vector + * @brief Set the gravity vector + * + * @param gravityVector The gravity vector. */ void setGravityVector(const std::array& gravityVector); + // =========== // GET METHODS // =========== /** - * Set the name of the robot + * @brief Get the name of the robot * - * @return Name of the robot + * @return Name of the robot. */ const std::string& getRobotName() const; /** - * Set the name of the file containing the urdf model + * @brief Get the name of the file containing the urdf model * - * @return Name of the file containing the urdf model + * @return Name of the file containing the urdf model. */ const std::string& getUrdfFile() const; /** - * Set the subset of controlled joints + * @brief Get the subset of controlled joints * - * @return Subset of controlled joints + * @return Subset of controlled joints. */ const std::vector& getControlledJoints() const; /** - * Set the names of the used ControlBoard names + * @brief Get the names of the used ControlBoard names * - * @return Names of the used ControlBoard names + * @return Names of the used ControlBoard names. */ const std::vector& getControlBoardsNames() const; /** - * Set the prefix appended to the opened ports. A leading "/" is always present. + * @brief Get the prefix appended to the opened ports. A leading "/" is always present. * - * @return Prefix appended to the opened ports + * @return Prefix appended to the opened ports. */ const std::string& getLocalName() const; /** - * Get a string with a unique identifier, generated from the name of the config - * block from Simulink. It might be useful when yarp ports must have a unique prefix - * (e.g. two RemoteControlBoardRemappers which share a ControlBoard) + * @brief Generate a unique identifier * - * @return the unique identifier + * Get a string with a unique identifier generated from the name of the config block from + * Simulink. It might be useful when yarp ports must have a unique prefix (e.g. two + * RemoteControlBoardRemappers which share a ControlBoard) + * + * @return The unique identifier. * @see setLocalName */ const std::string getUniqueId() const; /** - * Set the gravity vector + * @brief Get the gravity vector * - * @return The gravity vector + * @return The gravity vector. */ const std::array& getGravityVector() const; /** * Get the configured number of DoFs * - * @return The configured number of DoFs + * @return The configured number of DoFs. */ const size_t& getNumberOfDoFs() const; + /** + * @brief Get the unique identifier of this configuration object + * + * @return The unique identifier. + */ const std::string getConfKey() const { return m_confKey; } + // ============= // OTHER METHODS // ============= /** - * Checks if the stored configuration is valid, i.e. all the required fields have - * been saved successfully + * @brief Check if the congiguration is valid + * + * I.e. all the required fields have been stored successfully. * - * @return True if the configuration is valid + * @return True if the configuration is valid. */ bool isValid() const; + // ===================== // OPERATORS OVERLOADING // ===================== diff --git a/toolbox/include/base/Log.h b/toolbox/include/base/Log.h index 0fb4b2747..bea2427d5 100644 --- a/toolbox/include/base/Log.h +++ b/toolbox/include/base/Log.h @@ -34,7 +34,9 @@ namespace wbt { } // namespace wbt /** - * Basic Log class + * @brief Class for handling log messages + * + * Errors and Warnings are currently supported. */ class wbt::Log { @@ -45,13 +47,18 @@ class wbt::Log WARNING }; +private: + /** + * @brief Define the verbosity of the logs + * + * The verbosity is changed automatically detecting if the class is compiled in Debug. + */ enum LogVerbosity { RELEASE, DEBUG }; -private: std::vector m_errorsSStream; std::vector m_warningsSStream; @@ -63,19 +70,54 @@ class wbt::Log Log() = default; ~Log() = default; + /** + * @brief Get the Log singleton + * + * There is only one instance in the whole program of this class. + * + * @return The log singleton. + */ static wbt::Log& getSingleton(); + /** + * @brief Get the stringstream object for adding log messages + * + * @param type The log type. + * @param file The file from which this method is called (preprocessor directive). + * @param line The line from which this method is called (preprocessor directive). + * @param function The function from which this method is called (preprocessor directive). + * @return The stringstream object matching the log type. + */ std::stringstream& getLogStringStream(const LogType& type, const std::string& file, const unsigned& line, const std::string& function); + /** + * @brief Get the stored error messages. + * @return The error messages. + */ std::string getErrors() const; + + /** + * @brief Get the stored warning messages. + * @return The warning messages. + */ std::string getWarnings() const; + /** + * @brief Clear the stored error messages. + */ void clearErrors(); + + /** + * @brief Clear the stored warning messages. + */ void clearWarnings(); + /** + * @brief Clear all the stored log messages. + */ void clear(); }; diff --git a/toolbox/include/base/Parameter.h b/toolbox/include/base/Parameter.h index 7ad22837d..aa0987956 100644 --- a/toolbox/include/base/Parameter.h +++ b/toolbox/include/base/Parameter.h @@ -19,6 +19,10 @@ namespace wbt { enum class ParameterType; } // namespace wbt +/** + * @brief Defines the types of parameters supported by wbt::Parameter + * @see wbt::ParameterMetadata, wbt::Parameter + */ enum class wbt::ParameterType { // Scalar / Vector / Matrix @@ -41,6 +45,14 @@ enum class wbt::ParameterType STRUCT_CELL_DOUBLE, STRUCT_CELL_STRING }; + +/** + * @brief Class for storing parameter metadata + * + * A metadata must be constructed with an index and a name, and they cannot be changed afterwards. + * + * @see wbt::Parameter + */ class wbt::ParameterMetadata { public: @@ -70,6 +82,16 @@ class wbt::ParameterMetadata inline bool operator!=(const ParameterMetadata& rhs) { return !(*this == rhs); } }; +/** + * @brief Class for storing a generic parameter + * + * A generic parameters can be either a scalar or a vector. Supported types are defined by the + * wbt::ParameterType enum.Use wbt::ParameterMetadata to set these information. + * + * @tparam The type of the container type. For vector parameters, T is the type of an element of the + * container. + * @see wbt::Parameters, wbt::ParameterMetadata + */ template class wbt::Parameter { diff --git a/toolbox/include/base/Parameters.h b/toolbox/include/base/Parameters.h index 4690a8001..16355c293 100644 --- a/toolbox/include/base/Parameters.h +++ b/toolbox/include/base/Parameters.h @@ -20,6 +20,13 @@ namespace wbt { const std::string PARAM_INVALID_NAME = {}; } // namespace wbt +/** + * @brief Class for storing block's parameters + * + * This class can contain scalar and vector parameters of the supported types. + * + * @see wbt::Parameter, wbt::ParameterMetadata, wbt::ParameterType + */ class wbt::Parameters { public: @@ -57,41 +64,147 @@ class wbt::Parameters Parameters() = default; ~Parameters() = default; + /** + * @brief Get the name of a stored parameter from its index. + * @param index The index of the parameter. + * @return The name if the parameter exists, wbt::PARAM_INVALID_NAME otherwise. + */ ParamName getParamName(const ParamIndex& index) const; + + /** + * @brief Get the index of a stored parameter from its name. + * @param name The name of the parameter. + * @return The index if the parameter exists, wbt::PARAM_INVALID_INDEX otherwise. + */ ParamIndex getParamIndex(const ParamName& name) const; + + /** + * @brief Get the number of stored parameters + * + * @return The number of stored parameters. + */ unsigned getNumberOfParameters() const; + /** + * @brief Check if a parameter with a given name is stored + * + * @param name The name of the parameter. + * @return True if the parameters exists, false otherwise. + */ bool existName(const ParamName& name) const; - // Scalar parameters + /** + * @brief Store a scalar parameter + * + * @tparam The type of the parameter to store. Despite this, the parameter get cast accordingly + * to its metadata. + * @param param The value of the scalar parameter to store. + * @param paramMetadata The metadata associated to the parameter to store. + * @return True for success, false otherwise. + */ template bool storeParameter(const T& param, const wbt::ParameterMetadata& paramMetadata); - // Vector parameters + /** + * @brief Store a vector parameter + * + * @tparam The type of the parameter to store. Despite this, the parameter get cast accordingly + * to its metadata. + * @param param The value of the vector parameter to store. + * @param paramMetadata The metadata associated to the parameter to store. + * @return True for success, false otherwise. + */ template bool storeParameter(const std::vector& param, const wbt::ParameterMetadata& paramMetadata); - // Generic + /** + * @brief Store a parameter + * + * @tparam The type of the parameter to store. + * @param parameter The parameter object to store. + * @return True for success, false otherwise. + * + * @see wbt::Parameter + */ template bool storeParameter(const Parameter& parameter); - // Scalar / Struct Fields + /** + * @brief Get a scalar parameter + * + * @tparam The type of the output argument + * @param name The name of the parameter. + * @param param[out] The variable where the parameter value will be stored. Data get cast + * internally, even for string to numeric types. + * @return True for success, false otherwise. + */ template bool getParameter(const ParamName& name, T& param) const; + /** + * @brief Get a vector parameter + * + * @tparam The type of the output argument + * @param name The name of the parameter. + * @param param[out] The variable where the parameter value will be stored. Data get cast + * internally, even for string to numeric types. + * @return True for success, false otherwise. + */ template bool getParameter(const ParamName& name, std::vector& param) const; + /** + * @brief Get all the integer parameters + * + * @return The integer parameters + */ std::vector> getIntParameters() const; + + /** + * @brief Get all the boolean parameters + * + * @return The boolean parameters + */ std::vector> getBoolParameters() const; + + /** + * @brief Get all the double parameters + * + * @return The double parameters + */ std::vector> getDoubleParameters() const; + + /** + * @brief Get all the string parameters + * + * @return The string parameters + */ std::vector> getStringParameters() const; + /** + * @brief Get the metadata associated to a stored parameter + * + * @param name The name of the parameter. + * @return The metadata associate with the parameter for success, a metadata with a stored name + * `dummy` otherwise. + */ wbt::ParameterMetadata getParameterMetadata(const ParamName& name); + /** + * @brief Helper function for creating wbt::Configuration objects + * + * This helper method checks if the object has all the parameters for creating a + * wbt::Configuration object. + * + * @param parameters The parameters set where to look. + * @return True if the requirements are met, false otherwise. + * + * @see ToolboxSingleton::storeConfiguration + */ static bool containConfigurationData(const wbt::Parameters& parameters); }; +// ============ // GETPARAMETER // ============ @@ -124,6 +237,7 @@ namespace wbt { std::vector& param) const; } // namespace wbt +// ============== // STOREPARAMETER // ============== diff --git a/toolbox/include/base/RobotInterface.h b/toolbox/include/base/RobotInterface.h index 2ef716ee7..c6b7589e9 100644 --- a/toolbox/include/base/RobotInterface.h +++ b/toolbox/include/base/RobotInterface.h @@ -60,18 +60,16 @@ namespace wbt { } // namespace wbt /** - * \struct wbt::YarpInterfaces RobotInterface.h + * @brief A container for yarp interfaces pointers * - * This struct contains shared_ptrs to the devices which are (lazy) asked from the blocks. + * The pointers are lazy-asked by wbt::RobotInterface and stored in this struct. * - * @note The shared_ptr is owned only by wbt::RobotInterface. All the blocks will receive a - * weak_ptr. - * @remark Right now only asking / setting the whole joint set of the current Configuration is + * @remark Right now only operating on the whole joint set of the current Configuration is * supported. If in the future the support of operating on a subset will be implemented, - * IPositionControl2 and IVelocityControl2 should be implemented. For the time being, + * `IPositionControl2` and `IVelocityControl2` should be implemented. For the time being, * a possible workaround for this situation is creating a new configuration block * containing only the reduced set in a deeper-hierarchy Simulink's subsystem. - * @see RobotInterface::getDevice + * @see RobotInterface::getInterface */ struct wbt::YarpInterfaces { @@ -88,14 +86,13 @@ struct wbt::YarpInterfaces }; /** - * \class wbt::RobotInterface RobotInterface.h + * @brief Class for handling model and robot resources * - * This class holds the configuration used by one or more blocks, and all the objects to operate - * with the specified robot (real or model). + * This class is a wrapper of yarp::dev::RemoteControlBoardRemapper and + * iDynTree::KinDynComputations. By filling information in a wbt::Configuration object, it provides + * a simple initialization and combined usage of these two resources. * - * @see wbt::Configuration - * @see wbt::YarpInterfaces - * @see iDynTree::KinDynComputations + * @see wbt::Configuration, wbt::YarpInterfaces */ class wbt::RobotInterface { @@ -116,51 +113,58 @@ class wbt::RobotInterface // Counters for resource allocation / deallocation unsigned m_robotDeviceCounter; + // ====================== // INITIALIZATION HELPERS // ====================== /** + * + * @brief Initialize the model + * * Initialize the iDynTree::KinDynComputations with the information contained - * in wbt::Configuration. It finds the urdf file and stores the object to operate on it. - * If the joint list contained in RobotInterface::m_config is not complete, it loads a - * reduced model of the robot + * in wbt::Configuration. It finds from the file system the urdf file and stores the object to + * operate on it. If the joint list contained in RobotInterface::m_config is not complete, it + * loads a reduced model of the robot. * - * @return True if success + * @return True if success, false otherwise. */ bool initializeModel(); /** - * Configure a RemoteControlBoardRemapper device in order to allow + * @brief Initialize the remote controlboard remapper + * + * Configure a yarp::dev::RemoteControlBoardRemapper device in order to allow * interfacing the toolbox with the robot (real or in Gazebo). * - * @return True if success + * @return True if success, false otherwise. */ bool initializeRemoteControlBoardRemapper(); + // ===================== // OTHER PRIVATE METHODS // ===================== /** + * @brief Map joints between iDynTree and Yarp indices + * * Creates the map between joints (specified as either names or idyntree indices) and * their YARP representation, which consist in a pair: Control Board index and joint index * inside the its Control Board. * - * @see getJointsMapString - * @see getJointsMapIndex + * @see RobotInterface::getJointsMapString, RobotInterface::getJointsMapIndex * - * @return True if the map has been created successfully + * @return True if the map has been created successfully, false otherwise. */ bool mapDoFs(); /** - * Create a RemoteControlBoard object for a given remoteName + * @brief Create a RemoteControlBoard object for a given remoteName * * @see mapDoFs * - * @param remoteName [in] Name of the remote from which the remote control board is be - * initialized - * @param controlBoard [out] Smart pointer to the allocated remote control board - * @return True if success + * @param remoteName Name of the remote from which the remote control board is be initialized + * @param[out] controlBoard Smart pointer to the allocated remote control board + * @return True if success, false otherwise. */ bool getSingleControlBoard(const std::string& remoteName, std::unique_ptr& controlBoard); @@ -177,66 +181,72 @@ class wbt::RobotInterface // =========== /** - * Get the current configuration + * @brief Get the stored configuration * - * @return A reference of the Configuration object containing the current configuraton + * @return A reference of the configuration this object refers to. */ const wbt::Configuration& getConfiguration() const; /** - * Get the map between model joint names and the YARP representation (Control Board and + * @brief Get the map between model joint names and the YARP representation (Control Board and * joint index) * - * @return The joint map + * @return The joint map. */ - const std::shared_ptr getJointsMapString(); + const std::shared_ptr getJointsMapString(); /** - * Get the map between model joint indices and the YARP representation (Control Board and + * @brief Get the map between model joint indices and the YARP representation (Control Board and * joint index) * - * @return The joint map + * @return The joint map. */ - const std::shared_ptr getJointsMapIndex(); + const std::shared_ptr getJointsMapIndex(); /** - * Get the map between model joint names and the index representing their relative ordering - * inside the controlledJoints vector relative to their ControlBoard. + * @brief Get the map between model joint names and the index representing their relative + * ordering inside the controlledJoints vector relative to their ControlBoard * - * @note For example, if the joints are - * \verbatim j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3 \endverbatim, the map links them to - * \verbatim 0 1 2 0 1 0 \end + * @remark For example, if the joints are `j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3`, the map + * links them to `0 1 2 0 1 0`. Note that joints are 0-indexed. * - * @return The joint map + * @return The joint map. */ - const std::shared_ptr getControlledJointsMapCB(); + const std::shared_ptr getControlledJointsMapCB(); /** - * Get the map between the ControlBoard index inside the RemoteControlBoardRemapper - * and the number of the controlled joints which belongs to it. + * @brief Get the map between the ControlBoard index inside the RemoteControlBoardRemapper + * and the number of the controlled joints belonging to it * - * @note For example, if the joints are - * \verbatim j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3 \endverbatim, the generated map is - * \verbatim {{0,3}{1,2}{2,1}} \endverbatim + * @remark For example, if the joints are + * ``` + * j1_cb1 j2_cb1 j3_cb1 j1_cb2 j2_cb2 j1_cb3 + * ``` + * the generated map is + * ``` + * {{0,3}{1,2}{2,1}} + * ``` * Note that the map key is 0-indexed. * - * @return The control board limit map + * @return The control board limit map. */ - const std::shared_ptr getControlBoardIdxLimit(); + const std::shared_ptr getControlBoardIdxLimit(); /** - * Get the object to operate on the configured model + * @brief Get the object to operate on the configured model * - * @return A \c shared_ptr to the KinDynComputations object + * @return A `shared_ptr` to the KinDynComputations object. */ const std::shared_ptr getKinDynComputations(); /** - * Get a \c weak_ptr to an interface from the RemoteControlBoardRemapper + * @brief Get a Yarp interface + * + * The interface is lazy-evaluated. The handling of the memory is not responbility of the + * caller. It is handled internally. * - * param interface [out] A \c weak_ptr to the interface - * @return True if the \c weak_ptr is valid - * @tparam T The type of interface + * @param[out] interface The object that will contain the pointer to the interface. + * @return True for success, false otherwise. */ template bool getInterface(T*& interface); @@ -245,26 +255,27 @@ class wbt::RobotInterface // =============== /** - * Handles the internal counter for using the RemoteControlBoardRemapper + * @brief Handle the internal counter for using the `RemoteControlBoardRemapper`. * * @attention All the blocks which need to use any of the interfaces provided by - * wbt::YarpInterfaces must call this function in their initialize() method. - * @see releaseRemoteControlBoardRemapper + * wbt::YarpInterfaces must call this function in their Block::initialize method. * - * @return True if success + * @return True if success, false otherwise. + * @see RobotInterface::releaseRemoteControlBoardRemapper */ bool retainRemoteControlBoardRemapper(); /** - * Handles the internal counter for using the RemoteControlBoardRemapper. After the call - * from the last instance which retained the object, the RemoteControlBoardRemapper and all - * the allocated drivers get destroyed. + * @brief Handle the internal counter for using the `RemoteControlBoardRemapper`. * - * @note On the contrary of retainRemoteControlBoardRemapper, this method is already - * called in wbt::~WBBlock - * @see retainRemoteControlBoardRemapper + * After the call from the last instance which retained the object, the + * `RemoteControlBoardRemapper` and all the allocated drivers get destroyed. + * + * @attention All the blocks which need to use any of the interfaces provided by + * wbt::YarpInterfaces must call this function in their Block::terminate method. * - * @return True if success + * @return True if success, false otherwise. + * @see RobotInterface::retainRemoteControlBoardRemapper */ bool releaseRemoteControlBoardRemapper(); }; diff --git a/toolbox/include/base/Signal.h b/toolbox/include/base/Signal.h index 50e4805dc..728770cf9 100644 --- a/toolbox/include/base/Signal.h +++ b/toolbox/include/base/Signal.h @@ -20,6 +20,14 @@ namespace wbt { enum class DataType; } // namespace wbt +/** + * @brief Defines allowed signal data types + * + * This enum defines the data types of signals that are handled by this toolbox. + * + * @note Currently only `DOUBLE` is fully implemented. + * @see Signal::Signal, BlockInformation::setInputPortType, BlockInformation::setOutputPortType + */ enum class wbt::DataType { DOUBLE, @@ -33,9 +41,38 @@ enum class wbt::DataType BOOLEAN, }; +/** + * @brief Class to represent data shared between blocks, labelled as signals. + * + * Analogously to the block-algorithm corrispondence, this class introduces the signal-data + * corrispondence. Signals are basically the connections between blocks. + * + * Signals do not directly translate to block's input and output. Signals are plugged to block + * ports, and this block port fill the signal with data. + * + * @remark A signal can be plugged to more than one block port. + * @see wbt::Block + */ class wbt::Signal { public: + /// Defines the format of signals supported by Signal. It specifies what kind of data the + /// Signal::m_bufferPtr points. + /// + /// - `NONCONTIGUOUS` matches the default Simulink input signals. Signal::m_bufferPtr is a + /// pointer to an array of pointers, each of them storing an element of the signal. + /// - `CONTIGUOUS` means that the Signal::m_bufferPtr points to a contiguous array of + /// Signal::m_portDataType type. + /// - `CONTIGUOUS_ZEROCOPY` matches the default Simulink output signals. Signal::m_bufferPtr is + /// a pointer to an _external_ array of Signal::m_portDataType type. + /// + /// @note `CONTIGUOUS_ZEROCOPY` is the only format that doesn't copy data from the original + /// buffer address. Instead, both `CONTIGUOUS` and `NONCONTIGUOUS` data formats copy + /// the content of the buffer inside the Signal object. For performance reason, prefer + /// using `CONTIGUOUS_ZEROCOPY`. + /// @see initializeBufferFromContiguous, initializeBufferFromContiguousZeroCopy, + /// initializeBufferFromNonContiguous + /// @see BlockInformation::getInputPortSignal, BlockInformation::getOutputPortSignal enum class DataFormat { NONCONTIGUOUS = 0, @@ -49,7 +86,7 @@ class wbt::Signal const DataType m_portDataType; const DataFormat m_dataFormat; - void* m_bufferPtr; + void* m_bufferPtr; ///< Pointer to the data buffer @see DataFormat void deleteBuffer(); void allocateBuffer(const void* const bufferInput, void*& bufferOutput, unsigned length); @@ -57,37 +94,169 @@ class wbt::Signal public: static const int DynamicSize; - // Ctor and Dtor Signal(const DataFormat& dataFormat = DataFormat::CONTIGUOUS_ZEROCOPY, const DataType& dataType = DataType::DOUBLE, const bool& isConst = true); ~Signal(); - // Copy + Signal(const Signal& signal); Signal& operator=(const Signal& signal) = delete; - // Move + Signal(Signal&& signal); Signal& operator=(Signal&& signal) = delete; + /** + * @brief Initialize the signal from a contiguous buffer + * + * This method allocates a new array with the same size of `buffer` and copies the data. In this + * case, the Signal object will own the data. + * + * @param buffer The pointer to the original contiguous buffer. + * @return True for success, false otherwise. + * @see Signal::DataFormat + */ bool initializeBufferFromContiguous(const void* buffer); + + /** + * @brief Initialize the signal from a contiguous buffer without copying data + * + * This methods accepts an external contiguous buffer and holds its pointer. The data is not + * owned by this object. + * + * @note You must set the signal width with Signal::setWidth in order to have a valid signal. + * + * @param buffer The pointer to the original contiguous buffer. + * @return True for success, false otherwise. + * @see Signal::DataFormat + */ bool initializeBufferFromContiguousZeroCopy(const void* buffer); + + /** + * @brief Initialize the signal from a non-contiguous buffer + * + * This method allocates a new array with the same size of `bufferPtrs` and copies the data. In + * this case, the Signal object will own the data. `bufferPtrs` points to an array of pointers. + * Each of these pointers points to a data element. + * + * @note You must set the signal width with Signal::setWidth in order to have a valid signal. + * + * @param buffer The pointer to the original non-contiguous buffer. + * @return True for success, false otherwise. + * @see Signal::DataFormat + */ bool initializeBufferFromNonContiguous(const void* const* bufferPtrs); + /** + * @brief Check if the signal is valid + * + * Checks if Signal::m_bufferPtr is not `nullptr` and Signal::m_width is greater than zero. + * + * @return True for valid signal, false otherwise. + */ bool isValid() const; + /** + * @brief Check if the object a constant signal + * + * A constant Signal object can only read data from its buffer. It is widely used for input + * signals. + * + * @return True if the signal is constant, false otherwise. + */ bool isConst() const; + + /** + * @brief Read the width of the signal + * + * By default the width of Signal is Signal::DynamicSize. However, for being a valid signal, an + * object must have a specified width. + * + * @return The signal width. + * @see Signal::setWidth, Signal::isValid + */ int getWidth() const; + + /** + * @brief Read the wbt::DataType of the signal + * + * The default type is DataType::DOUBLE. + * + * @return The signal data type. + */ DataType getPortDataType() const; + + /** + * @brief Read the Signal::DataFormat of the signal + * + * The default type is DataFormat::CONTIGUOUS_ZEROCOPY. + * + * @return The signal data format. + */ DataFormat getDataFormat() const; + /** + * @brief Get the pointer to the buffer storing signal's data + * + * The buffer is stored as a void pointer in Signal::m_bufferPtr. In order to use the buffer it + * should be properly cast to the right data type. Be sure that the Signal::DataType match the + * type of the buffer otherwise pointer arithmetics does not work. + * + * If `T` does not match Signal::m_portDataType the returned value is a `nullptr`. + * + * @note Always check if the pointer is not `nullptr` before using it. + * @tparam The data type of the returned buffer. + * @return The pointer to the buffer if the class is properly configured, `nullptr` otherwise. + * @see Signal::setBuffer + */ template T* getBuffer() const; + /** + * @brief Get a single element of the signal + * + * This method returns the `i-th` element of the handled buffer. + * + * @note It is recommended to use Signal::isValid before using this method. + * @tparam The data type of the returned signal. It must match Signal::m_portDataType. + * @param i The index of the element. It should not exceed Signal::m_width. + * @return The `i-th` element of the vector if the signal is valid, or the default value of the + * type otherwise. + */ template T get(const unsigned& i) const; + /** + * @brief Set the width of the signal + * + * @param width The width to set. + */ void setWidth(const unsigned& width); + + /** + * @brief Set the value of a sigle element of the buffer + * + * @param index The index of the element to write. + * @param data The content of the data to write. + * @return True for success, false otherwise. + * + * @todo Port this to a template + */ bool set(const unsigned& index, const double& data); + + /** + * @brief Set the pointer to the buffer storing signal's data + * + * This method allows changing the handled buffer. In the DataFormat::CONTIGUOUS case the data + * is copied inside the object. Instead, in the DataFormat::CONTIGUOUS_ZEROCOPY only the pointer + * to the buffer is changed. + * + * This method is not allowed for DataFormat::NONCONTIGUOUS format. + * + * @tparam The data type of the new buffer. + * @param data The new buffer address. + * @param length The size of the new buffer. + * @return True if the buffer was set sucessfully, false otherwise. + */ template bool setBuffer(const T* data, const unsigned& length); }; diff --git a/toolbox/include/base/ToolboxSingleton.h b/toolbox/include/base/ToolboxSingleton.h index f600b7fb4..07d434ffd 100644 --- a/toolbox/include/base/ToolboxSingleton.h +++ b/toolbox/include/base/ToolboxSingleton.h @@ -89,8 +89,6 @@ class wbt::ToolboxSingleton */ static wbt::ToolboxSingleton& sharedInstance(); - // TODO: check if doxygen ref work - /** * Returns the Configuration object labelled by confKey. * This object is contained into the wbt::RobotInterface object. @@ -134,13 +132,24 @@ 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 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); /** diff --git a/toolbox/include/base/WBBlock.h b/toolbox/include/base/WBBlock.h index 4fd84b99b..81aad1f37 100644 --- a/toolbox/include/base/WBBlock.h +++ b/toolbox/include/base/WBBlock.h @@ -35,10 +35,7 @@ namespace iDynTree { } // namespace iDynTree /** - * \struct iDynTreeRobotState WBBlock.h - * - * This struct contains the iDynTree objects used to configure the - * state of iDynTree::KinDynComputations objects. + * @brief Container for data structures used for using `iDynTree::KinDynComputations::setRobotState` */ struct wbt::iDynTreeRobotState { @@ -55,32 +52,64 @@ struct wbt::iDynTreeRobotState }; /** - * Basic class for Whole-Body related blocks. - * This class (the whole toolbox in reality) assumes the block represent - * an instantaneous system (i.e. not a dynamic system). + * @brief Extension of wbt::Block for simplifying the development of whole-body blocks + * + * This class provides support of parsing the parameters for creating a wbt::RobotInterface object, + * and helpers for retrieving iDynTree::KinDynComputations and wbt::RobotInterface objects. + * + * @see wbt::Block * - * You can create a new block by deriving this class and implementing at least - * the output method. + * @section wbblock_parameters WBBlock Parameters * - * This block implements the following default behaviours: - * - it ask for 4 parameters (robot name, local (module) name, names of the remote control boards, - * and the list of joints which should all belong to one of the remote control boards) - * - It initializes the yarp network and the whole body interface object - * - During terminate it closes and release the interface object and terminate the yarp network + * In addition to @ref block_parameters, wbt::WBBlock requires: * - * @note Usually you want to call this class implementations at some point in your - * method overridings, unless you want to completely change the code (but at that point - * you probabily want to derive from Block instead) + * | Type | Index | Rows | Cols | Name | + * | ---- | :---: | :---: | :---: | ----- | + * | ::PARAM_STRUCT_STRING | 0 + Block::NumberOfParameters | 1 | 1 | "RobotName" | + * | ::PARAM_STRUCT_STRING | 0 + Block::NumberOfParameters | 1 | 1 | "UrdfFile" | + * | ::PARAM_STRUCT_CELL_STRING | 0 + Block::NumberOfParameters | 1 | 1 | "ControlledJoints" | + * | ::PARAM_STRUCT_CELL_STRING | 0 + Block::NumberOfParameters | 1 | 1 | "ControlBoardsNames" | + * | ::PARAM_STRUCT_STRING | 0 + Block::NumberOfParameters | 1 | 1 | "LocalName" | + * | ::PARAM_STRUCT_DOUBLE | 0 + Block::NumberOfParameters | 1 | 3 | "GravityVector" | + * | ::PARAM_STRING | 1 + Block::NumberOfParameters | 1 | 1 | "ConfBlockName" | + * + * @note The first set of parameters are fields of the same struct. For this reason they share the + * same index. */ class wbt::WBBlock : public wbt::Block { protected: wbt::iDynTreeRobotState m_robotState; + /** + * @brief Helper for retrieving the iDynTree::KinDynComputations object from + * wbt::BlockInformation + * @param blockInfo A BlockInformation object. + * @return A pointer to iDynTree::KinDynComputations. + */ std::weak_ptr getKinDynComputations(const BlockInformation* blockInfo) const; + + /** + * @brief Helper for retrieving the wbt::RobotInterface object from + * wbt::BlockInformation + * @param blockInfo A BlockInformation object. + * @return A pointer to wbt::RobotInterface. + */ std::weak_ptr getRobotInterface(const BlockInformation* blockInfo) const; + /** + * @brief Helper for setting the robot state inside the iDynTree::KinDynComputations object + * + * @param basePose The vector containing the base pose. + * @param jointsPos The vector containing the joints positions. + * @param baseVelocity The vector containing the base velocity. + * @param jointsVelocity The vector containing the joints velocities. + * @param kinDyn A pointer to the block's KinDynComputations object. + * @return True if success, false otherwise. + * + * @see iDynTree::KinDynComputations::setRobotState, wbt::iDynTreeRobotState + */ bool setRobotState(const wbt::Signal* basePose, const wbt::Signal* jointsPos, const wbt::Signal* baseVelocity, @@ -88,6 +117,7 @@ class wbt::WBBlock : public wbt::Block iDynTree::KinDynComputations* kinDyn); public: + /// The number of parameters WBBlock requires static const unsigned NumberOfParameters; WBBlock() = default;