WB-Toolbox 3.0 #56

diegoferigo · 2017-09-11T07:38:09Z

Refer to #46 and this milestone for the task list.

This PR:

Fixes Configure library without yarpWholeBodyInterface.ini file #53 Configure library without yarpWholeBodyInterface.ini file
Fixes Substitute yarpWholeBodyActuators by interfacing to Yarp controlboards #52 Substitute yarpWholeBodyActuators by interfacing to Yarp controlboards
Fixes Substitute yarpWholeBodyModel with iDynTree #51 Substitute yarpWholeBodyModel with iDynTree
Fixes Directly obtain joint velocities from control board #50 Directly obtain joint velocities from control board
Fixed Multiple robots in the same simulink model #37 Multiple robots in the same simulink model

[WIP]

traversaro · 2017-09-11T07:50:25Z

toolbox/include/CentroidalMomentum.h

+    iDynTree::Vector3*       m_gravity;
+    iDynTree::Transform*     m_world_T_base;
+    iDynTree::VectorDynSize* m_jointsVelocity;
+    iDynTree::VectorDynSize* m_jointsState;


In mechanical systems, "state" is tipically used to indicate position and velocity, so I would call this member m_jointsPosition or something similar.

I didn't like the previous word configuration and I switched to State. Position is even more clear 👍

traversaro

Do you think it would make sense to add an explicitly call to setFrameVelocityRepresentation to make sure that the representation is the intended one?

traversaro · 2017-09-11T08:35:44Z

toolbox/include/DotJNu.h

+    class Twist;
+    class Transform;
+    class VectorDynSize;
+    typedef int FrameIndex;


I understand the concern of avoiding too many includes to reduce compilation times, but I think redefining a typedef like this, as it will create problems if we have change the definition of FrameIndex.
However, if you prefer it like this no problem for me.

I tried to keep as much as possible the same behavior as the current state, and I agree with you on these forward declaration. For VectorFixSize I had to add the header since redefining templates was a bit too much imo, but applying this line to all the typedefs would make sense too

traversaro · 2017-09-11T08:41:03Z

toolbox/src/CentroidalMomentum.cpp

-        m_jointsVelocity = new double[dofs];
-
-        return m_basePose && m_centroidalMomentum && m_basePoseRaw && m_configuration && m_baseVelocity && m_jointsVelocity;
+        double m_gravityRaw[3] {0, 0, -9.81};


I don't know where this is set at the moment, but for different reasons it could make sense to have this configurable at configure time/runtime eventually. A classical example is a fixed base robot mounted on a table, if the gravity acceleration of the table can be set from the outside it is trivial to compensate eventual skewness of the table.

So far the only block with an optional input for gravity is InverseDynamics. So far I tried to maintain the current functionalities, without the need of editing the Simulink's masks. I'll add this in a future TODO list

traversaro · 2017-09-11T08:44:23Z

toolbox/src/DotJNu.cpp

        }
-
+        else {
+            m_frameIndex = COM_LINK_ID;


The KinDynComputations class has separate methods to compute the COM position/jacobian/bias term. Passing a m_frameIndex of -1 (that is equivalent to iDynTree::FRAME_INVALID_INDEX` will result in a runtime error.

@traversaro In the output() method of all the blocks that use COM_LINK_ID the correct iDynTree's method for CoM is called. Since all integers <0 are treated as FRAME_INVALID_INDEX by iDynTree, do you have any suggestion on how to handle this case with a viarable?

I mean, when m_frameIndex = -1 and the frame is com, an error is thrown. If m_frameIndex = -1 and the frame is not com, the block goes further to the output() and the COM_LINK_ID is used to to select the alternative method for computations on CoM.

I see, I missed the check on COM_LINK_ID . Everything should work fine indeed, but personally I would prefer to have a separated boolean variable to save the information "is this block computing COM-related stuff", rather than overloading the meaning of m_frameIndex . However, this is mostly a personal taste, so feel free to use the pattern that you prefer. The only advice that I would recommend is to define COM_LINK_ID to any negative number different from -1 , as -1 is exactly the value of iDynTree::FRAME_INVALID_INDEX.

I like the boolean variable idea. It is the most safe and clean in the long run

traversaro · 2017-09-11T08:45:17Z

toolbox/src/DotJNu.cpp

+            // OUTPUT
+            // ======
+
+            // Compute the dot{J}*dot{q}


We typically prefer \nu over dot{q} .

traversaro · 2017-09-11T08:46:17Z

toolbox/src/ForwardKinematics.cpp

-        success = success && blockInfo->setInputPortMatrixSize(0, 4, 4);
-        success = success && blockInfo->setInputPortVectorSize(1, dofs);
+        success &= blockInfo->setInputPortMatrixSize(INPUT_IDX_BASE_POSE, 4, 4);
+        success &= blockInfo->setInputPortVectorSize(INPUT_IDX_JOINTCONF, dofs);


Not a bit fan of the use of bit field operations to represent compactly boolean operations, but I know it works so if for you it is ok it is ok also for me. : )

I found the previous version redundant, if @francesco-romano has no comments on this I'd like to use the new one.

honestly I am against the use of &=

traversaro · 2017-09-11T08:53:04Z

toolbox/src/MassMatrix.cpp

+            // =======================
+            model->setRobotState(*m_world_T_base,
+                                 *m_jointsState,
+                                 *m_baseVelocity,


By default for performance reason iDynTree::VectorFixSize objects are not initialized to any value, so this call will effectively use not initialized memory. I suggest to explicitly add a call to m_baseVelocity->zero() in the initialization method to all the vector that you expect to be set to zero.
The VectorDynSize are indeed zeroed during initialization, but I suggest to add an explicit to zero() also for them for consistency and protect you from possible future changes in iDynTree .

diegoferigo · 2017-09-11T15:22:28Z

Do you think it would make sense to add an explicitly call to setFrameVelocityRepresentation to make sure that the representation is the intended one?

@traversaro Can you please give me some further detail on this? What representation are we using inside the Toolbox? I'm a bit confused on this.

SetReferences subList parameter was erroneously converted to a boolean, while it is actually the index of the selected item in the Simulink mask dropdown. Being the index >=1 the boolean conversion resulted always in a true value. Fix robotology#57

Note: actually the std::cerr are warnings for the user, this is why they are not returned as an error. We have to find a smarter way to return the warning to the user.

diegoferigo · 2017-11-08T16:26:49Z

@francesco-romano @traversaro
First draft of the WB-Toolbox 3.0 pushed. For the time being only C++, ready for the first round of review.

traversaro · 2017-11-08T16:29:46Z

toolbox/include/base/BlockInformation.h

-     * @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 [in] option implementation-specific block option


Is this an in or out parameter?

An out parameter, I don't even remember why I changed this

francesco-romano · 2017-11-09T08:36:49Z

I think we use Mixed representation, am I right @traversaro ?

francesco-romano

As discussed F2F I reviewed until 0e99c6f (included)

francesco-romano · 2017-11-09T09:53:56Z

+WBToolbox/WBToolboxConfig.m

+
+    properties
+        % Robot state
+        RobotName          (1,:) char


why capital case letters?

Is this a MATLAB convention?

francesco-romano · 2017-11-09T09:55:15Z

+WBToolbox/WBToolboxConfig.m

+        % Dependent properties
+        % --------------------
+
+        function set.ValidConfiguration(~,~)


I do not know anything of MATLAB classes but, isn't possible to have "read only" properties, i.e. only public getter?
This would avoid to declare a fake set method

ValidConfiguration is a Dependent property, so by default even without this set method an error is generated. Specifying this method gives the possibility to provide a custom error message, and in this case it is used exactly for this reason.

francesco-romano · 2017-11-09T09:56:56Z

+WBToolbox/WBToolboxConfig.m

+                ~isempty(obj.ControlBoardsNames) && ...
+                ~isempty(obj.LocalName) && ...
+                ~isequal(obj.GravityVector,[0, 0, 0]);
+            % Check if the urdf file has the right extension


I am not sure that the extension is actually needed. I would drop this requirement.
It is then iDynTree that will validate the URDF file.

Ok, let's be generic then

francesco-romano · 2017-11-09T09:58:34Z

MxAnyType/CMakeLists.txt

+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+include(GNUInstallDirs)
+option(BUILD_SHARED_LIBS OFF)


This line is not needed.

francesco-romano · 2017-11-09T09:59:25Z

MxAnyType/CMakeLists.txt

+
+# Find Eigen
+find_package(Eigen3 REQUIRED)
+target_include_directories(${VARS_PREFIX} PUBLIC "${EIGEN3_INCLUDE_DIR}")


actually, if Eigen is used only in cpp, this can have a PRIVATE include

Ping @traversaro. Has Eigen been fixed on Windows when used in headers? Any comment / alternatives on this?

What do you mean by "been fixed"? And how this is related to PRIVATE/PUBLIC?

@traversaro the actual question is: Eigen members in header files.
I know there were issues with exported symbols in Windows. Furthermore, I do not like them in header files (as they force you to have Eigen)

Replied in #56 (comment) .

Final solution #56 (comment)

francesco-romano · 2017-11-09T11:31:39Z

toolbox/src/base/Signal.cpp

    {
        if (isConstPort) return;
        unsigned dataSize = sizeof(bool);
        const void* address = static_cast<const bool*>(data) + startIndex;

-        memcpy(contiguousData, address, dataSize * length);
+        memcpy(contiguousData, address, dataSize* length);


same as before

francesco-romano · 2017-11-09T11:33:16Z

toolbox/include/base/ToolboxSingleton.h

+    // ========================
+
+    ToolboxSingleton();
+    ToolboxSingleton(const ToolboxSingleton&);


If it is a singleton both copy and assignment should be prohibited

Used = delete

francesco-romano · 2017-11-09T11:34:05Z

toolbox/include/base/ToolboxSingleton.h

+     * @return        A \c shared_ptr to the iDynTree::KinDynComputations of the requested
+     *                configuration
+     */
+    const std::shared_ptr<iDynTree::KinDynComputations> model(const std::string& confKey);


To be consistent with the previous two methods I would rename this into getModel

This was an inheritance of the previous version, changed the name. This method might even be removed, it's never called. Right now the best way to gather the model is asking it directly from the parent WBBlock using getRobotInterface()->getKinDynComputations(). This allows the block not to know the existence of the confKey variable.

francesco-romano · 2017-11-09T11:34:19Z

toolbox/include/base/ToolboxSingleton.h

+     * @return         Returns \c true if configure is successful, \c false otherwise
+     * @see            ToolboxSingleton::isKeyValid
+     */
+    bool configure(const std::string& confKey, const Configuration& config);


as before, I do not like the name configure

Renamed to storeConfiguration()

francesco-romano · 2017-11-09T11:38:57Z

toolbox/src/base/ToolboxSingleton.cpp

+        //       This may cause problems if the config block's mask is changed after the first compilation.
+        if (m_interfaces.find(confKey) == m_interfaces.end()) {
+            m_interfaces[confKey] = std::make_shared<RobotInterface>(config);
+            return m_interfaces[confKey].get();


This comment applies also to other uses of .get().
shared_ptr have an operator bool. This means that you can directly return return m_interfaces[confKey] to assess validity of the pointer.

Change this line and the other times you use get() as a boolean check

The conversion from shared_ptr to bool is explicit. An alternative to get() would be static_cast<bool>. Using this latter version.

This class contains and validates the configuration passed to every wbt::WBBlock object. ValidConfiguration is a Dependent property and it is set to 1 only if the basic checks currently implemented in other properties don't fail. From this state, it is possible to call getSimulinkParameters() in order to obtain the struct which is passed to the S-Functions and then parsed in C++. This class also contains utilities functions for obtainng a simple string representation of 1D vectors and 1D chars, required to serialize the data in the Config Block's mask. The deserialization is performed using eval().

Initial version of the library, which will become independent in the future. MxAnyType handles the C / C++ mxArray pointer that represents any generic data type in Matlab. It wraps most of the C APIs, and it has optional support of strict type checking.

traversaro · 2017-11-10T14:13:46Z

MxAnyType/CMakeLists.txt

+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+include(GNUInstallDirs)
+option(BUILD_SHARED_LIBS OFF)
+set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON)


This variable only make sense if you are building shared libraries.

Yes, in the future this will become a shared library. For the time being it is static, but just for convenience.

wbt::Log is the new class that provides log support to the WBToolbox. It now supports both Errors and Warnings. For being more flexible, wbt::Log is a singleton. This approach simplifies dramatically the handling of warnings, and can be called from every method without the need of passing it though arguments. Since singletons cannot be deleted, wbt::Log state is reset in the early stage of the processing. [Squash] toolbox.cpp

Its functionality are implemented by the new generic WBBlock, which is a refactored version of WBIBlock that access robot resources in a lazy way. This approach allows avoiding the previous separation between WBIBlock and WBIModelBlock.

This class contains the data passed from Simulink. It matches the data structure of Matlab's WBToolboxConfig.getSimulinkParameters() struct.

This class contains all the required data structures to operate on a robot, either through the model or the interfaces to the hardware / gazebo. From the Simulink point of view, there is one instance of RobotInterface for every Configuration block added in the model. In fact, RobotInterface extends by composition the Configuration object. It handles the pointers to its members in a lazy way, initializing on the fly the asked resource.

This is a temporary fix. The cpp file should be generated by its thrift source, which should be found already installed in the system. A new CMake option e.g. USE_GAZEBO should be added.

This commit fixes also some error in computing the address used in memcpy calls

Furthermore: - All methods that read parameters return a bool value - optionFromKey doesn't use anymore wbt::Data - setNumberOfOuputPorts() became setNumberOfOutputPorts()

This new WBBlock implements the same functionalities of the former WBIBlock and WBIModelBlock blocks. Resources are lazy-handled, and it is not anymore necessary having two different setups. Furthermore: - The parameters from Simulink are stored in the ToolboxSingleton in the configureSizeAndPorts() instead of initialize(). This is possible because singletons are not deleted by Simulink between these two steps (Blocks instead are deleted).

- Removed old Error parameter from methods - Small documentation changes - Small style edits

The new ToolboxSingleton class contains in a persistent wat Configuration and RobotInterface objects. Most of the logic previously contained in WBInterface has been moved in these two new classes.

Kept as private variables only what is strictly required

Now it works as expected when the initial state vector is set but is not enabled

It is still not clear what is the Matlab version which introduced this feature. For now I am reverting it, addressing robotology#66.

traversaro reviewed Sep 11, 2017

View reviewed changes

traversaro requested changes Sep 11, 2017

View reviewed changes

diegoferigo force-pushed the WB3.0 branch from 48ebac8 to 3fb4df9 Compare September 11, 2017 15:16

francesco-romano added 3 commits September 25, 2017 12:10

Migrated old DH conversion code from embedded to iDynTree

288d6e9

Temporary fix: included iostream

6716aaa

Note: actually the std::cerr are warnings for the user, this is why they are not returned as an error. We have to find a smarter way to return the warning to the user.

diegoferigo force-pushed the WB3.0 branch from 3fb4df9 to 0dd4603 Compare November 8, 2017 16:21

traversaro reviewed Nov 8, 2017

View reviewed changes

francesco-romano suggested changes Nov 9, 2017

View reviewed changes

diegoferigo mentioned this pull request Nov 10, 2017

Templates for data types #60

Closed

diegoferigo added 2 commits November 10, 2017 13:05

traversaro reviewed Nov 10, 2017

View reviewed changes

diegoferigo added 14 commits November 10, 2017 15:37

Removed WBIModelBlock

b4beb26

Its functionality are implemented by the new generic WBBlock, which is a refactored version of WBIBlock that access robot resources in a lazy way. This approach allows avoiding the previous separation between WBIBlock and WBIModelBlock.

New Configuration class

aee4eed

This class contains the data passed from Simulink. It matches the data structure of Matlab's WBToolboxConfig.getSimulinkParameters() struct.

ClockServer thrift messages are now a static library

af1fa41

This is a temporary fix. The cpp file should be generated by its thrift source, which should be found already installed in the system. A new CMake option e.g. USE_GAZEBO should be added.

Substituted C-style cast with static_cast in Signal.cpp

1d71a86

This commit fixes also some error in computing the address used in memcpy calls

Switch to AnyType interface to handle data read by BlockInformation

19cf4d6

Furthermore: - All methods that read parameters return a bool value - optionFromKey doesn't use anymore wbt::Data - setNumberOfOuputPorts() became setNumberOfOutputPorts()

Minor edits of base/ classes

04531f1

- Removed old Error parameter from methods - Small documentation changes - Small style edits

WBInterface refactored and renamed to ToolboxSingleton

639fb71

The new ToolboxSingleton class contains in a persistent wat Configuration and RobotInterface objects. Most of the logic previously contained in WBInterface has been moved in these two new classes.

CentroidalMomentum refactored

42a06f4

DotJNu refactored

aab17af

ForwardKinematics refactored

2881b1b

GetLimits refactored

881f382

diegoferigo added 6 commits December 12, 2017 09:10

First working version of DiscreteFilter block with 1D I/O signal

2a8397e

DiscreteFilter: Multidimesional signal support

4d6317b

DiscreteFilter: Included parameters for initializing input / output

1ff1fb0

Kept as private variables only what is strictly required

DiscreteFilter: improved behavior of optional parameters

2c54e8e

Now it works as expected when the initial state vector is set but is not enabled

Reduced memory allocations

aae5cff

Rebased DiscreteFilter block on WB3.0

bc51e1d

diegoferigo force-pushed the WB3.0 branch from 47f3b5a to bc51e1d Compare December 12, 2017 09:27

diegoferigo mentioned this pull request Dec 12, 2017

DiscreteFilter block #47

Closed

gabrielenava mentioned this pull request Jan 19, 2018

How to setup the enviroment for running torque control on iCub robotology/whole-body-controllers#10

Closed

diegoferigo added 2 commits January 25, 2018 16:53

Disabled size initialization of matlab class' properties

ae4234f

It is still not clear what is the Matlab version which introduced this feature. For now I am reverting it, addressing robotology#66.

Small edits to README.md

198e44b

diegoferigo force-pushed the WB3.0 branch from c471d2c to 198e44b Compare January 25, 2018 17:06

diegoferigo self-assigned this Jan 25, 2018

diegoferigo merged commit be84398 into robotology:WB3.0 Jan 25, 2018

diegoferigo mentioned this pull request Feb 22, 2018

WB-Toolbox 3.0 Release #76

Merged

diegoferigo added complexity::hard pr::resolution::merged priority::normal type::enhancement component::blocks component:library-mex component:simulink labels Feb 27, 2018

diegoferigo mentioned this pull request Jun 22, 2018

New CMake structure and plugin-based blocks architecture #137

Merged

traversaro mentioned this pull request Dec 22, 2018

Synchronisation problems between Gazebo and estimation threads #26

Closed

WB-Toolbox 3.0 #56

WB-Toolbox 3.0 #56

Conversation

diegoferigo commented Sep 11, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

diegoferigo Sep 11, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

diegoferigo commented Sep 11, 2017

diegoferigo commented Nov 8, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

francesco-romano commented Nov 9, 2017

francesco-romano left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

diegoferigo Nov 10, 2017 • edited Loading

Choose a reason for hiding this comment

diegoferigo commented Sep 11, 2017 •

edited

Loading

diegoferigo Sep 11, 2017 •

edited

Loading

diegoferigo commented Nov 8, 2017 •

edited

Loading

diegoferigo Nov 10, 2017 •

edited

Loading