Control single pendulum with idle and torque modes

[xela-95]

The goal of this issue is to have a working MVP in which the single pendulum can be controlled in two modes: idle and torque.

In order to enable this functionality, we need to port from gazebo-yarp-plugins the controlboard plugin, with only the minimal subset of features to make this MVP work.

Additional references:

• Single pendulum tutorial in gazebo-yarp-plugins
• Gazebo Sim API useful to set force/torque
• Corresponding invocation in gazebo-yarp-plugins ControlBoardDriver.cpp

The controlboard plugin for gazebo classic implements several interfaces, only a subset of them are necessary for make this MVP work, see https://github.com/robotology/gz-sim-yarp-plugins/issues/77#issuecomment-1919576062:

• [x] DeviceDriver
• [x] IEncodersTimed (inherits IEncoders)
• [x] IInteractionMode
• [x] IControlMode
• [x] ITorqueControl
• [x] IAxisInfo (dummy)
• [x] IControlLimits
• [x] IPositionControl (dummy implementation)
• [x] IVelocityControl (dummy implementation)
• [x] IRemoteVariables (dummy implementation)
• [x] IPidControl (dummy implementation)
• [x] ICurrentControl (dummy implementation)

[xela-95]

CC @traversaro

[xela-95]

Debug a gazebo-yarp-plugin using gdb and vscode

In order to understand how the ControlBoard plugin works and to select a minimal subset of functionalities needed by the single pendulum example I'm trying to launch in debug the gazebo-yarp-plugins.

My setup is the following:

• conda environment defined in project README
• gz-sim-yarp-plugins compiled in Debug configuration
• Visual Studio Code with C/C++ Extension Pack.

The ideal workflow is to attach a debugger (like gdb in Linux to the Gazeob server process and from there being able to pause the code execution at breakpoints, and being able to step forward the execution, check the value of class data members, analyze the call stack, etc.

To debug a plugin from vscode it is necessary to define a debug configuration in a launch.json file (see the official documentation for more details), like the following:

{
    "version": "0.2.0",
    "configurations": [
        {
            "name": "(gdb) Attach",
            "type": "cppdbg",
            "request": "attach",
            "processId": "${input:GetPID}",
            "program": "${input:GetPath}",
            "MIMode": "gdb",
            "setupCommands": [
                {
                    "description": "Enable pretty-printing for gdb",
                    "text": "-enable-pretty-printing",
                    "ignoreFailures": true
                },
                {
                    "description": "Set Disassembly Flavor to Intel",
                    "text": "-gdb-set disassembly-flavor intel",
                    "ignoreFailures": true
                }
            ]
        }
    ],
    "inputs": [
        {
            "id": "GetPID",
            "type": "command",
            "command": "shellCommand.execute",
            "args": {
                "command": "pgrep gzserver",
                "description": "Select your target PID",
                "useFirstResult": true,
            }
        },
        {
            "id": "GetPath",
            "type": "command",
            "command": "shellCommand.execute",
            "args": {
                "command": "readlink /proc/$(pgrep gzserver)/exe",
                "description": "Select your target PID",
                "useFirstResult": true,
            }
        }
    ]
}

This launch configuration has been taken from this blog post made for ROS2 Gazebo plugins.

Unfortunately, to make it work, it requires Task Shell Input extension to be installed on vscode, since AFAIK it is not possible to launch custom shell commands from this configuration files (more information here).

Finally, the process to launch the debugging is the following:

1. Add breakpoints to the code of the plugin you want to debug;
2. From a terminal in which the environment is active, launch gazebo;
3. Launch the debugger from vscode, by pressing F5 or Run -> Debug from the menu bar (if you have multiple debug configurations defined for your project, select the one defined above from the Run and Debug panel, like in the image below:
4. Add the model you are interested in; in this case the single pendulum;
5. The plugin should be instantiated, and the execution should be paused on the defined breakpoint:
6. From there you can start debugging as you prefer.

[xela-95]

CC @giotherobot @diegoferigo

[xela-95]

The ControlBoardDriver class implements a lot of interfaces and base classes:

https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h#L83C1-L102C1

For this issue I think that probably the interfaces to implement are (EDITED):

Inherited interfaces/base classes	Needed for MVP
DeviceDriver	✅
IPositionControl	✅ (dummy implementation)
IVelocityControl	✅ (dummy implementation)
IAmplifierControl
IEncodersTimed	✅
IControlCalibration2
IControlLimits
IInteractionMode	✅
DeviceResponder
IControlMode	✅
ITorqueControl	✅
IPositionDirect
IImpedanceControl
IPWMControl
ICurrentControl
IPidControl
IRemoteVariables
IAxisInfo	✅ (probably)
IVirtualAnalogSensor

[traversaro]

I think we can skip the IVirtualAnalogSensor for the MVP. Instead for sure we need IAxisInfo and IEncodersTimed.

[traversaro]

There are a bunch of interfaces that we need to at least implement with dummy methods (even just methods that do not do anything) IPositionControl and IVelocityControl, as otherwise the controlBoard_nws_yarp will not start: https://github.com/robotology/yarp/blob/c09c9f6d6e2f4c9c9fb3acf845459090c19652c0/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp#L343-L354 .

[xela-95]

@traversaro while I'm trying to implement the control board interfaces I'm thinking on how we have implemented the data passing via the singleton class in the other plugins.

For example in the basestate plugin (the others already ported follow the same strategy) its singleton returns a raw pointer to the struct BaseData. I think that this is not a safe way to return data, since the memory should be managed carefully and segfaults can happen if the data member is deleted from the map. Upgrading to a shared_pointer could be safer? Are there other preferred ways to do this? I thought that maybe at least for this new plugin I can start with a more safer solution that then I will port to the other plugins already implemented.

[xela-95]

My other doubt concerns how we store data for each plugin. For sensor plugins (FT, lidar, IMU, ...), we store in the singleton map the data correspondent to each plugin instance using as the key the sensor scoped name, for model plugins in the case of base state we used the (base) link scoped name. For control board, if I understood well we can have a 1-to-may relationship between robot and control boards, and a 1-to-1 relationship between control board and joint. Should hence the key be the joint scoped name?

[traversaro]

Upgrading to a shared_pointer could be safer? Are there other preferred ways to do this? I thought that maybe at least for this new plugin I can start with a more safer solution that then I will port to the other plugins already implemented.

I am not sure a shared_ptr provide any advantage in this context. shared_ptr is useful when you want to model shared ownership of an object, in the sense that as soon as the shared_ptr goes out of scope in all the code that uses it, the instance is destroyed. In a singleton case, the idea is memory never destroyed, so I am not sure if the shared_ptr provide any advantage.

[traversaro]

Upgrading to a shared_pointer could be safer? Are there other preferred ways to do this? I thought that maybe at least for this new plugin I can start with a more safer solution that then I will port to the other plugins already implemented.

I am not sure a shared_ptr provide any advantage in this context. shared_ptr is useful when you want to model shared ownership of an object, in the sense that as soon as the shared_ptr goes out of scope in all the code that uses it, the instance is destroyed. In a singleton case, the idea is memory never destroyed, so I am not sure if the shared_ptr provide any advantage.

Ah no, I got it what you mean, just store shared_ptr in the map instead of plain pointers. Yes, that may be tangentially better, even if I am a bit harder to detect if a device is removed from the singleton, as the code consuming the shared_ptr will continue to have a valid shared_ptr, that however will not be updated anymore.

[traversaro]

Should hence the key be the joint scoped name?

No, each controlboard wraps many joints, so I am not sure that would work. My idea was to create a key combining the model scoped name and the name of the instance of the plugin, i.e. the name attribute of plugin tag, for example in:

      <plugin name="gzyarp::IMU" filename="gz-sim-yarp-imu-system">

However, I am not sure if the name attribute needs to be unique, or if it is accessible anyhow. The alternative is to add some custom key that we can access, or combine all the joint name somehow to create a key, but that seems brittle.

[xela-95]

@traversaro looking at the DriverCreatorOf<>() for the controlboard plugin in https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoard.cc#L98C1-L102C15 I do not understand what I should use as wrap argument. The documentation states that is The "common name" of another device which can wrap this device up for network access. If there is no such device, use an empty name: "", but it is not so clear to me.

[traversaro]

It is the default Network Wrapper Server (see https://www.yarp.it/latest//group__nws__and__nwc__architecture.html) that should be used for the device, when launched by yarpdev --device <devicename> . However:

• yarpdev is seldomly used anymore, as it has been almost superseded by the yarprobotinterface, in which all Network Wrapper Server are launched explicitly.
• We are adding a device to the factory via yarp::dev::Drivers::factory().add(), meaning that we can only invoke it in the process in which we added it to the factory. So that parameter, that is only considered when invoking the device via yarpdev, will never be used in this context.

For these two reasons, it is perfectly fine to just leave that argument empty.

[xela-95]

@traversaro I have a doubt regarding the namespaces used to define the plugin types.

In the classes implementing the Gazebo plugin interfaces the namespace is gzyarp (like here ForceTorque, while for the classes implementing Yarp interfaces, these are the classes to which the Driver suffix is added are in the yarp::dev::gzyarp namespace (see here for ForceTorqueDriver.

Is it a good practice to have the same namespace name gzyarp but within different namespaces? I was a bit confused at the beginning because I thought these classes were implemented in the same namespace :sweat_smile:

[traversaro]

I think it is just a leftover from my old confusion, that got propagated until now. Feel free to use gzyarp for both.

[xela-95]

@traversaro I'm thinking on how to achieve a good data structure for the control board (eventually to apply also to the other plugins)(follow up the reasoning of https://github.com/robotology/gz-sim-yarp-plugins/issues/77#issuecomment-1921308773).

In the picture below I've drawn an ugly schema of the current data structure:

The main entities are:

• Gazebo Class: this class implement all the gazebo interfaces needed to configure the plugin, set states before the simulation stes, read the simulation outputs, manage resets, ...
• Driver class: this class implements all the yarp interfaces of the plugin, like DeviceDriver and all the others necessary for the particular plugin;
• Singleton: this class implements the singleton pattern, i.e. it can exist only one instance of it. It contains a map for all the possible instances of this type of plugin (for example all the control boards in use during a simulation).
• Data structure: is a struct (for now at least) containing the actual data for each instance of the device (read control board in this case). There is an instance of it for each entry of the singleton map.

The main question is how to access to read and set values of this data structure from both the Gazebo class and Driver class in a thread-safe and memory-safe point of view.

There are two mutexes that came into play: one for the singleton class, whenever we need to get or set an entry of the map and another one for the data structure.

In the other plugins implementations each Driver class instance has as private data member a raw pointer to its data structure and it access directly its fields to get and set data whenever requested. The same happens for the Gazebo class. Is it a good way to retain directly a pointer to the data structure contained into the dictionary of the singleton? Is it better to ask every time we need it to the get method of the singleton?

Second: is it good practice to implement a lock every time we need to get or set a member of the data structure? in the control board there are tens of such methods, maybe such access to the resource can be encapsulated in some way?

(sorry for the ramblings 😆 )

[xela-95]

@traversaro I was looking at the IAxisInfo interface, and I've seen that it deals with the coupling of joints (using methods from IJointCoupling. I think that maybe it's too complex to implement this now, I'm thinking of providing dummy implementations of the IAxisInfo interface.

If from you there are things to keep in mind related to these concepts let me know.

[traversaro]

@traversaro I was looking at the IAxisInfo interface, and I've seen that it deals with the coupling of joints (using methods from IJointCoupling. I think that maybe it's too complex to implement this now, I'm thinking of providing dummy implementations of the IAxisInfo interface.

If from you there are things to keep in mind related to these concepts let me know.

No need to go into coupling for now, just return the names with getAxisName and the type (that you can hardcode to revolute for now with getJointType.

[xela-95]

@traversaro I was looking at the IAxisInfo interface, and I've seen that it deals with the coupling of joints (using methods from IJointCoupling. I think that maybe it's too complex to implement this now, I'm thinking of providing dummy implementations of the IAxisInfo interface. If from you there are things to keep in mind related to these concepts let me know.

No need to go into coupling for now, just return the names with getAxisName and the type (that you can hardcode to revolute for now with getJointType.

Perfect thanks!

[xela-95]

No need to go into coupling for now, just return the names with getAxisName and the type (that you can hardcode to revolute for now with getJointType.

Regarding the axes names, looking at https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h#L422 I cannot find where they are defined. Are they different from the joint names, right?

[traversaro]

No need to go into coupling for now, just return the names with getAxisName and the type (that you can hardcode to revolute for now with getJointType.

Regarding the axes names, looking at https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/include/yarp/dev/ControlBoardDriver.h#L422 I cannot find where they are defined. Are they different from the joint names, right?

They are the same. Axes is just a YARP-specific name, they may be different in the context of coupled joints, but if we stick to simple joint structure they are the same.

[xela-95]

@traversaro I've converted the old model.urdf to an SDF file by using the gazebo utility cli:

gz sdf -p model.urdf > model.sdf

, then I took the model part and used it inside the SDF world for the single pendulum tutorial: https://github.com/robotology/gz-sim-yarp-plugins/blob/568d82023d82b97c0fd1f4f7a7728d9e07c3fc68/tutorial/single_pendulum/world.sdf#L67-L155

By launching gazebo with the sdf file the single pendulum appears:

I never used this tool and I don't know well the urdf and sdf specifications, so if you find something that has to be corrected please let me know.

[traversaro]

That seems to be fine. In theory you should be able to insert the Gazebo plugin inside an URDF model like in the tutorial of gazebo-yarp-plugins, for now we can start simple and directly use the SDF you converted.

[xela-95]

@traversaro I'm encountering strange behaviors while trying to setup the tutorial of the single pendulum. In commit a3ff6d5 I've created a world sdf file and inside it I included the single_pendulum model in the same way I've read from documentation. I created a database.config file in which the model directory of the single pendulum is listed, and this folder I put the model.sdf, model.config and the conf folder. I've then set up $GAZEBO_MODEL_PATH to point to the models folder.

The strange thing is that I'm only able to refer to the single pendulum model from the world.sdf using the model://single_pendulum notation only when the world file is in the parent directory of the model, if the world file is everywhere else it is not possible anymore. I don't know if this is a requirement of Gazebo, but I could not find any documentation about it.

The same applies to the path of the configuration files of the model.

However this is not a blocking issue at the moment.

[xela-95]

A blocking issue instead is when I launch the single pendulum world file I obtain the following error:

$ gz sim single_pendulum_world.sdf --verbose
[Msg] Gazebo Sim GUI    v7.6.0
[Msg] Received world [single_pendulum_world.sdf] from the GUI.
[Msg] Gazebo Sim Server v7.6.0
[Msg] Loading SDF world file[/home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum_world.sdf].
[Msg] Serving entity system service on [/entity/system/add]
[Msg] Create service on [/world/turorial_controlboard/create]
[Msg] Remove service on [/world/turorial_controlboard/remove]
[Msg] Pose service on [/world/turorial_controlboard/set_pose]
[Msg] Pose service on [/world/turorial_controlboard/set_pose_vector]
[Msg] Light configuration service on [/world/turorial_controlboard/light_config]
[Msg] Physics service on [/world/turorial_controlboard/set_physics]
[Msg] SphericalCoordinates service on [/world/turorial_controlboard/set_spherical_coordinates]
[Msg] Enable collision service on [/world/turorial_controlboard/enable_collision]
[Msg] Disable collision service on [/world/turorial_controlboard/disable_collision]
[Msg] Material service on [/world/turorial_controlboard/visual_config]
[Msg] Material service on [/world/turorial_controlboard/wheel_slip]
Error while loading the library [/home/acroci/mambaforge/envs/gz7/lib/libgz-sim-yarp-controlboard-system.so]: /home/acroci/mambaforge/envs/gz7/lib/libgz-sim-yarp-controlboard-system.so: undefined symbol: _ZN6gzyarp25ControlBoardDataSingleton5mutexEv
[Err] [SystemLoader.cc:141] Failed to load system plugin: (Reason: No plugins detected in library)
- Requested plugin name: [gzyarp::ControlBoard]
- Requested library name: [gz-sim-yarp-controlboard-system]
- Resolved library path: [/home/acroci/mambaforge/envs/gz7/lib/libgz-sim-yarp-controlboard-system.so]
[DEBUG] Reading file /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/single_pendulum_nws.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/single_pendulum_nws.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  9.53674e-07 s
[INFO] startup phase starting...
[INFO] Opening device single_pendulum_nws_yarp with parameters [("robotName" = "single_pendulum"), ("name" = "/singlePendulumGazebo/body"), ("period" = "0.01")]
[DEBUG] |yarp.dev.PolyDriver|single_pendulum_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id single_pendulum_nws_yarp) (name "/singlePendulumGazebo/body") (period 0.0100000000000000002082) (robotName single_pendulum)
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/rpc:i| Port /singlePendulumGazebo/body/rpc:i active at tcp://10.240.2.37:10002/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/command:i| Port /singlePendulumGazebo/body/command:i active at tcp://10.240.2.37:10003/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/state:o| Port /singlePendulumGazebo/body/state:o active at tcp://10.240.2.37:10004/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/stateExt:o| Port /singlePendulumGazebo/body/stateExt:o active at tcp://10.240.2.37:10005/
[INFO] |yarp.dev.PolyDriver|single_pendulum_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device single_pendulum_nws_yarp with parameters [("device" = "controlboard_plugin_device")]
[ERROR] Target device controlboard_plugin_device (network = ... ) does not exist.
[ERROR] Cannot run attach action on device single_pendulum_nws_yarp
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[WARNING] There was some problem running actions for startup phase . Please check the log and your configuration
[INFO] startup phase finished.
[ERROR] gz-sim-yarp-robotinterface-system : impossible to start robotinterface
[INFO] interrupt1 phase starting...
[INFO] interrupt1 phase finished.
[INFO] shutdown phase starting...
[INFO] Entering action level 5 of phase shutdown
[INFO] Executing detach action, level 5 on device single_pendulum_nws_yarp with parameters []
[INFO] All actions for action level 5 of shutdown phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of shutdown phase finished.
[INFO] Closing device single_pendulum_nws_yarp
[INFO] shutdown phase finished.

The libgz-sim-yarp-controlboard-system.so is installed correctly, but the error message for the undefined symbol: _ZN6gzyarp25ControlBoardDataSingleton5mutexEv is strange...

I'm now trying to debug this.

[traversaro]

The actual problem seems to be:

Error while loading the library [/home/acroci/mambaforge/envs/gz7/lib/libgz-sim-yarp-controlboard-system.so]: /home/acroci/mambaforge/envs/gz7/lib/libgz-sim-yarp-controlboard-system.so: undefined symbol: _ZN6gzyarp25ControlBoardDataSingleton5mutexEv [Err] [SystemLoader.cc:141] Failed to load system plugin: (Reason: No plugins detected in library) - Requested plugin name: [gzyarp::ControlBoard] - Requested library name: [gz-sim-yarp-controlboard-system] - Resolved library path: [/home/acroci/mambaforge/envs/gz7/lib/libgz-sim-yarp-controlboard-system.so]

It seems a linking problem. You can demangle _ZN6gzyarp25ControlBoardDataSingleton5mutexEv via the C++ demangler website and check if that variable is properly defined, and the file in which it is defined is added to the library in CMake .

[xela-95]

It seems a linking problem. You can demangle _ZN6gzyarp25ControlBoardDataSingleton5mutexEv via the C++ demangler website and check if that variable is properly defined, and the file in which it is defined is added to the library in CMake .

Thank you, didn't know about demangling, I used http://demangler.com/ and it was really useful in order to point me towards missing definitions of data members and functions.

However, I expected that such missing definitions should be prompted as errors to me by the IDE, instead that didn't occur (only going over the declaration a message appeared saying that no definition could be found) and the build succeeded.

With commit https://github.com/robotology/gz-sim-yarp-plugins/pull/78/commits/2fc4edc5dce98c2260a5e9b046145be3892e36ec Gazebo starts without the error given in https://github.com/robotology/gz-sim-yarp-plugins/issues/77#issuecomment-1929285442, however there are other issues I'm investigating, related to configurations files not found and errors in yarprobotinterface.

[INFO] gz-sim-yarp-controlboard-system: configuration of device  controlboard_plugin_device  loaded from yarpConfigurationFile :  /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/gazebo_controlboard.ini 
[ERROR] |yarp.os.YarpPluginSettings| Cannot find "model/single_pendulum/controlboard_plugin_device" plugin (not built in, and no .ini file found for it)Check that YARP_DATA_DIRS leads to at least one directory with plugins/model/single_pendulum/controlboard_plugin_device.ini or share/yarp/plugins/model/single_pendulum/controlboard_plugin_device.ini in it
[ERROR] |yarp.dev.PolyDriver|model/single_pendulum/controlboard_plugin_device| Could not find device <model/single_pendulum/controlboard_plugin_device>
[ERROR] gz-sim-yarp-controlboard-system Plugin failed: error in opening yarp driver
[DEBUG] Reading file /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/single_pendulum_nws.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/single_pendulum_nws.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  4.76837e-07 s
[INFO] startup phase starting...
[INFO] Opening device single_pendulum_nws_yarp with parameters [("robotName" = "single_pendulum"), ("name" = "/singlePendulumGazebo/body"), ("period" = "0.01")]
[DEBUG] |yarp.dev.PolyDriver|single_pendulum_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id single_pendulum_nws_yarp) (name "/singlePendulumGazebo/body") (period 0.0100000000000000002082) (robotName single_pendulum)
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/rpc:i| Port /singlePendulumGazebo/body/rpc:i active at tcp://10.240.2.37:10002/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/command:i| Port /singlePendulumGazebo/body/command:i active at tcp://10.240.2.37:10003/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/state:o| Port /singlePendulumGazebo/body/state:o active at tcp://10.240.2.37:10004/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/stateExt:o| Port /singlePendulumGazebo/body/stateExt:o active at tcp://10.240.2.37:10005/
[INFO] |yarp.dev.PolyDriver|single_pendulum_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device single_pendulum_nws_yarp with parameters [("device" = "controlboard_plugin_device")]
[ERROR] Target device controlboard_plugin_device (network = ... ) does not exist.
[ERROR] Cannot run attach action on device single_pendulum_nws_yarp
[INFO] All actions for action level 5 of startup phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of startup phase finished.
[WARNING] There was some problem running actions for startup phase . Please check the log and your configuration
[INFO] startup phase finished.
[ERROR] gz-sim-yarp-robotinterface-system : impossible to start robotinterface
[INFO] interrupt1 phase starting...
[INFO] interrupt1 phase finished.
[INFO] shutdown phase starting...
[INFO] Entering action level 5 of phase shutdown
[INFO] Executing detach action, level 5 on device single_pendulum_nws_yarp with parameters []
[INFO] All actions for action level 5 of shutdown phase started. Waiting for unfinished actions.
[INFO] All actions for action level 5 of shutdown phase finished.
[INFO] Closing device single_pendulum_nws_yarp
[INFO] shutdown phase finished.

[diegoferigo]

If I don't mistake, you should be able to spot these problems earlier by using the following linker flags:

https://github.com/robotology/gym-ignition/blob/4bc8e0465eed68911434485ba0b9c0a6740d516e/scenario/CMakeLists.txt#L37-L45

[xela-95]

@traversaro I'm not understanding what it's missing for the example to run. Everything is the same to the gazebo-yarp-plugins example, except for the fact that I do not instantiate a default NWS as discussed in https://github.com/robotology/gz-sim-yarp-plugins/issues/77#issuecomment-1923356154.

I've found that the error message

[ERROR] |yarp.os.YarpPluginSettings| Cannot find "model/single_pendulum/controlboard_plugin_device" plugin (not built in, and no .ini file found for it)Check that YARP_DATA_DIRS leads to at least one directory with plugins/model/single_pendulum/controlboard_plugin_device.ini or share/yarp/plugins/model/single_pendulum/controlboard_plugin_device.ini in it

comes from https://github.com/robotology/yarp/blob/55d8319d4278b7c8de22895fa09c21b83bd87e16/src/libYARP_os/src/yarp/os/YarpPlugin.cpp#L203C26-L218 , while the error

[ERROR] |yarp.dev.PolyDriver|model/single_pendulum/controlboard_plugin_device| Could not find device <model/single_pendulum/controlboard_plugin_device>

comes from https://github.com/robotology/yarp/blob/55d8319d4278b7c8de22895fa09c21b83bd87e16/src/libYARP_dev/src/yarp/dev/PolyDriver.cpp#L210

[traversaro]

The device spawned by the PolyDriver factory is specified by the device argument, and you specify as device the scoped name, see https://github.com/robotology/gz-sim-yarp-plugins/pull/78/files#diff-b9bab8988652feec7498da624856c6d3f347c268f3276c00bad45c33315d13aeR89 . You should instead specify the device name as you added it to the Factory in https://github.com/robotology/gz-sim-yarp-plugins/pull/78/files#diff-b9bab8988652feec7498da624856c6d3f347c268f3276c00bad45c33315d13aeR50, so gazebo_controlboard.

[xela-95]

The device spawned by the PolyDriver factory is specified by the device argument, and you specify as device the scoped name, see https://github.com/robotology/gz-sim-yarp-plugins/pull/78/files#diff-b9bab8988652feec7498da624856c6d3f347c268f3276c00bad45c33315d13aeR89 . You should instead specify the device name as you added it to the Factory in https://github.com/robotology/gz-sim-yarp-plugins/pull/78/files#diff-b9bab8988652feec7498da624856c6d3f347c268f3276c00bad45c33315d13aeR50, so gazebo_controlboard.

Thank you, that solved the issue. Unfortunately, the way in which the configuration properties are used in yarp is still a bit obscure to me. Now I'm facing a new error, related to the fact that there are no joints detected for the model, I think this is also related to other properties configured erroneously:

[INFO] gz-sim-yarp-controlboard-system: configuration of device  controlboard_plugin_device  loaded from yarpConfigurationFile :  /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/gazebo_controlboard.ini 
[DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (GAZEBO_PIDS (Pid0 1000.0 2.0 0.100000000000000005551 9999 9999 9 9)) (GAZEBO_VELOCITY_PIDS (Pid0 500.0 2.0 0.100000000000000005551 9999 9999 9 9)) (device gazebo_controlboard) (disableImplicitNetworkWrapper) (initialConfiguration "0.0 0.0") (jointNames joint) (name "model/single_pendulum/controlboard_plugin_device") (robotScopedName "model/single_pendulum") (yarpDeviceName controlboard_plugin_device)
==========> m_controlBoardScopedName: model/single_pendulum
[INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class ControlBoardDriver for documentation.
[INFO] Registered YARP device with instance name: model/single_pendulum/controlboard_plugin_device
[DEBUG] Reading file /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/single_pendulum_nws.xml
[DEBUG] yarprobotinterface: using xml parser for DTD v3.x
[DEBUG] Reading file /home/acroci/repos/gz-sim-yarp-plugins/tutorial/models/single_pendulum/conf/single_pendulum_nws.xml
[INFO] Yarprobotinterface was started using the following enable_tags: 
[INFO] Yarprobotinterface was started using the following disable_tags: 
[DEBUG] List of all enable attributes found in the include tags: 
[DEBUG] List of all disable attributes found in the include tags: 
[DEBUG] Preprocessor complete in:  4.76837e-07 s
[INFO] startup phase starting...
[INFO] Opening device single_pendulum_nws_yarp with parameters [("robotName" = "single_pendulum"), ("name" = "/singlePendulumGazebo/body"), ("period" = "0.01")]
[DEBUG] |yarp.dev.PolyDriver|single_pendulum_nws_yarp| Parameters are (device controlBoard_nws_yarp) (id single_pendulum_nws_yarp) (name "/singlePendulumGazebo/body") (period 0.0100000000000000002082) (robotName single_pendulum)
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/rpc:i| Port /singlePendulumGazebo/body/rpc:i active at tcp://10.240.79.27:10002/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/command:i| Port /singlePendulumGazebo/body/command:i active at tcp://10.240.79.27:10003/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/state:o| Port /singlePendulumGazebo/body/state:o active at tcp://10.240.79.27:10004/
[INFO] |yarp.os.Port|/singlePendulumGazebo/body/stateExt:o| Port /singlePendulumGazebo/body/stateExt:o active at tcp://10.240.79.27:10005/
[INFO] |yarp.dev.PolyDriver|single_pendulum_nws_yarp| Created wrapper <controlBoard_nws_yarp>. See C++ class ControlBoard_nws_yarp for documentation.
[INFO] Entering action level 5 of phase startup
[INFO] Executing attach action, level 5 on device single_pendulum_nws_yarp with parameters [("device" = "controlboard_plugin_device")]
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: iJointFault interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IPidControl interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IPositionControl interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IPositionDirect interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IVelocityControl interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IMotor interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IMotorEncoders interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IAmplifierControl interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IControlLimits interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IControlCalibration interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IImpedanceControl interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IPreciselyTimed interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IRemoteVariables interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: IPWMControl interface was not found in subdevice.
[WARNING] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: ICurrentControl interface was not found in subdevice.
[ERROR] |yarp.devices.controlBoard_nws_yarp| Part </singlePendulumGazebo/body>: attached device has an invalid number of joints (0)

[traversaro]

The error comes from https://github.com/robotology/yarp/blob/55d8319d4278b7c8de22895fa09c21b83bd87e16/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp#L356-L359, and tmp_axes is actually populated by the getAxes call in https://github.com/robotology/yarp/blob/55d8319d4278b7c8de22895fa09c21b83bd87e16/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp#L333 . So we need to make sure that at least getAxes works fine, i.e. that https://github.com/robotology/gz-sim-yarp-plugins/pull/78/files#diff-7b78d4fa28c11f70eb0d5d0c6978eb8ec0d9ddeae84d77e65d5569f763ba21b7R319 is called and return the intended value. Indeed, if I look in the code I do not see anything that either resize or push_back or change the size of ControlBoardDriver::m_controlBoardData->joints or ControlBoard::m_controlBoardData.joints anywhere, so I guess that it is kind of expected that its size is 0?

[xela-95]

The error comes from https://github.com/robotology/yarp/blob/55d8319d4278b7c8de22895fa09c21b83bd87e16/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp#L356-L359, and tmp_axes is actually populated by the getAxes call in https://github.com/robotology/yarp/blob/55d8319d4278b7c8de22895fa09c21b83bd87e16/src/devices/networkWrappers/controlBoard_nws_yarp/ControlBoard_nws_yarp.cpp#L333 . So we need to make sure that at least getAxes works fine, i.e. that https://github.com/robotology/gz-sim-yarp-plugins/pull/78/files#diff-7b78d4fa28c11f70eb0d5d0c6978eb8ec0d9ddeae84d77e65d5569f763ba21b7R319 is called and return the intended value. Indeed, if I look in the code I do not see anything that either resize or push_back or change the size of ControlBoardDriver::m_controlBoardData->joints or ControlBoard::m_controlBoardData.joints anywhere, so I guess that it is kind of expected that its size is 0?

Thank you @traversaro that pointed me towards the right direction. What are the expectations in terms of when the configuration could change? It has to be read only at startup (and reset) or we should allow to modify the configuration files at runtime and be able to update the control board properties (like the joints associated) accordingly?

[traversaro]

Just at startup, even reset does not re-read the configuration files.

[xela-95]

Ok finally the startup phase is working, both the ControlBoard and the NWS devices are initialized correctly (or at least there are not evident errors logged).

Now, launching the yarpmotorgui I get the following expected errors, since some other interface implementations are missing (https://github.com/robotology/gz-sim-yarp-plugins/issues/77#issuecomment-1920063164):

$ yarpmotorgui                                                                                    
[WARNING] robotDescriptionServer not found, robot parts will be set manually.
[DEBUG] Appending /singlePendulumGazebo/body
[DEBUG] Checking the existence of: /yarpmotorgui0//singlePendulumGazebo/body/rpc:o 
[DEBUG] ADDRESS is:  
[DEBUG] |yarp.dev.PolyDriver|remote_controlboard| Parameters are (carrier udp) (device remote_controlboard) (local "/yarpmotorgui0//singlePendulumGazebo/body") (remote "/singlePendulumGazebo/body")
[INFO] |yarp.os.Port|/yarpmotorgui0//singlePendulumGazebo/body/rpc:o| Port /yarpmotorgui0//singlePendulumGazebo/body/rpc:o active at tcp://10.240.2.37:10006/
[INFO] |yarp.os.Port|/yarpmotorgui0//singlePendulumGazebo/body/command:o| Port /yarpmotorgui0//singlePendulumGazebo/body/command:o active at tcp://10.240.2.37:10007/
[INFO] |yarp.os.Port|/yarpmotorgui0//singlePendulumGazebo/body/stateExt:i| Port /yarpmotorgui0//singlePendulumGazebo/body/stateExt:i active at tcp://10.240.2.37:10008/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/yarpmotorgui0//singlePendulumGazebo/body/rpc:o| Sending output from /yarpmotorgui0//singlePendulumGazebo/body/rpc:o to /singlePendulumGazebo/body/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/yarpmotorgui0//singlePendulumGazebo/body/command:o| Sending output from /yarpmotorgui0//singlePendulumGazebo/body/command:o to /singlePendulumGazebo/body/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/yarpmotorgui0//singlePendulumGazebo/body/stateExt:i| Receiving input from /singlePendulumGazebo/body/stateExt:o to /yarpmotorgui0//singlePendulumGazebo/body/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|remote_controlboard| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[DEBUG] Setting a valid finder 
[INFO] |yarp.os.Port|/yarpmotorgui/body/sequence:o| Port /yarpmotorgui/body/sequence:o active at tcp://10.240.2.37:10009/
[DEBUG] Initializing interfaces...
[DEBUG] Opening interfaces...
[INFO] body iencs->getEncoders() ok!
[ERROR] Error while getting position limits, part  body  joint  0
[ERROR] Error while getting velocity limits, part  body  joint  0
[ERROR] Error while getting current range, part  body  joint  0
[WARNING] Unable to update dutyCycles
[ERROR] Missing Implementation of getTargetPosition()
[ERROR] Missing Implementation of getRefVelocity()
[ERROR] Missing Implementation of getRefSpeed()
[ERROR] Missing Implementation of checkMotionDone()

[xela-95]

@traversaro I'm implementing the IControlLimits interface.

What are the units of measure of the units that are being set/get?

[traversaro]

I can't remember where this is document, but it is [deg] and [deg]/s for revolute joints, and m and m/s for prismatic joints. Gazebo APIs instead use radiangs for revolute joints, so you would need to handle that.

[xela-95]

Trying to address the errors I get when launch yarprobotinterface, I implemented the IRemoteVariables interface since I saw from the logs of yarprobotinterface that a remote_controlboard device is created. By implementing dummy methods of this interface, I see that now the getLimits, getVelLimits, getRefSpeed methods of ControlBoardDriver are being called. However, now I got the following error messages:

[ERROR] |yarp.devices.controlBoard_nws_yarp| I do not have a valid ICurrentControl interface
[ERROR] |yarp.devices.controlBoard_nws_yarp| I do not have a valid IPidControl interface

I'll try to provide dummy implementations also of these interfaces.

[xela-95]

With commit https://github.com/robotology/gz-sim-yarp-plugins/pull/78/commits/95fe846786fc267d504c7088948968e243f93551 I tried provide implementations for these interfaces, but some errors still remains from the yarprobotinterface side.:

[WARNING] robotDescriptionServer not found, robot parts will be set manually.
[DEBUG] Appending /singlePendulumGazebo/body
[DEBUG] Checking the existence of: /yarpmotorgui0//singlePendulumGazebo/body/rpc:o 
[DEBUG] ADDRESS is: tcp://yarpmotorgui0//singlePendulumGazebo/body/rpc:o 
[DEBUG] |yarp.dev.PolyDriver|remote_controlboard| Parameters are (carrier udp) (device remote_controlboard) (local "/yarpmotorgui1//singlePendulumGazebo/body") (remote "/singlePendulumGazebo/body")
[INFO] |yarp.os.Port|/yarpmotorgui1//singlePendulumGazebo/body/rpc:o| Port /yarpmotorgui1//singlePendulumGazebo/body/rpc:o active at tcp://10.240.2.37:10010/
[INFO] |yarp.os.Port|/yarpmotorgui1//singlePendulumGazebo/body/command:o| Port /yarpmotorgui1//singlePendulumGazebo/body/command:o active at tcp://10.240.2.37:10011/
[INFO] |yarp.os.Port|/yarpmotorgui1//singlePendulumGazebo/body/stateExt:i| Port /yarpmotorgui1//singlePendulumGazebo/body/stateExt:i active at tcp://10.240.2.37:10012/
[INFO] |yarp.os.impl.PortCoreOutputUnit|/yarpmotorgui1//singlePendulumGazebo/body/rpc:o| Sending output from /yarpmotorgui1//singlePendulumGazebo/body/rpc:o to /singlePendulumGazebo/body/rpc:i using tcp
[INFO] |yarp.os.impl.PortCoreOutputUnit|/yarpmotorgui1//singlePendulumGazebo/body/command:o| Sending output from /yarpmotorgui1//singlePendulumGazebo/body/command:o to /singlePendulumGazebo/body/command:i using udp
[INFO] |yarp.os.impl.PortCoreInputUnit|/yarpmotorgui1//singlePendulumGazebo/body/stateExt:i| Receiving input from /singlePendulumGazebo/body/stateExt:o to /yarpmotorgui1//singlePendulumGazebo/body/stateExt:i using udp
[INFO] |yarp.dev.PolyDriver|remote_controlboard| Created device <remote_controlboard>. See C++ class RemoteControlBoard for documentation.
[DEBUG] Setting a valid finder 
[INFO] |yarp.os.Port|/yarpmotorgui/body/sequence:o| Port /yarpmotorgui/body/sequence:o active at tcp://10.240.2.37:10009/
[DEBUG] Initializing interfaces...
[DEBUG] Opening interfaces...
[INFO] body iencs->getEncoders() ok!
[ERROR] Error while getting velocity limits, part  body  joint  0
[ERROR] Error while getting current range, part  body  joint  0
[WARNING] Unable to update dutyCycles
[ERROR] Missing Implementation of getTargetPosition()
[ERROR] Missing Implementation of getRefVelocity()
[WARNING] Unable to update dutyCycles

I don't know what is actually causing them and whether is possible to neglect these errors for the moment and focus on the actual implementation of the business logic to control in torque the pendulum.

[traversaro]

I think as long as the GUI starts, you can probably ignore the error messages.

[xela-95]

Hi @traversaro, I'm implenting the code to read the state of the joints after a simulation step. We need to read for each joint its position, velocity and torque.

In gazebo-yarp-plugins the snippet doing this is the following: https://github.com/robotology/gazebo-yarp-plugins/blob/cb3521f7543a5a07e2a65a82dedd2eb2ec2cea12/plugins/controlboard/src/ControlBoardDriver.cpp#L612-L626

In gz-sim the data structure is changed:

• Position
• Velocity
• TransmittedWrench

Both Position and Velocity methods return a std::vector< double >, while in the old approach the value was a single double. How to handle this data? Maybe a frame conversion is needed?

Moreover TransmittedWrench contains both force and torque members, each one being a std::vector< double > of three components (i.e. a 6D wrench). How to deal with this data?

[traversaro]

Both Position and Velocity methods return a std::vector< double >, while in the old approach the value was a single double. How to handle this data? Maybe a frame conversion is needed?

I am not sure, but I guess the size of that std::vector<double> is 1 for 1 dof joints such as revolute or prismatic?

[diegoferigo]

Some xref to old discussion we had a while ago:

• https://github.com/gazebosim/gz-physics/issues/124
• https://github.com/gazebosim/gz-physics/pull/283

[traversaro]

Moreover TransmittedWrench contains both force and torque members, each one being a std::vector< double > of three components (i.e. a 6D wrench). How to deal with this data?

This is a good question! It is related to this old gazebo-yarp-plugins issue https://github.com/robotology/gazebo-yarp-plugins/issues/471 and related gazebo-classic issue https://github.com/gazebosim/gazebo-classic/issues/1321 . I think we can finally fix this in gz-sim, by using the Equation 4.22 at page 84 of https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf or the equivalent Equation 3.37 at page 54 of Featherstone's Rigid Body Dynamics Algorithm . The TransmittedWrench is something similar to ${}_E \mathrm{f}_{E,F}$ . However, we can just have a placeholder for now, until we figure out a working software skeleton.

[traversaro]

Ah, I missed @diegoferigo comment, those are also really relevant issues.

[diegoferigo]

If we have a 6D force transmitted by the joint, I guess that if the frame is correct we can get the 1D joint torque by projecting it to the motion subspace. In other words, using the axis of the joint.

[traversaro]

Just to clarify, @diegoferigo's formulas are valid only if you are expressing the wrench in a frame that has its origin on the axis of the joint, otherwise they are a bit more complicated.

[xela-95]

Thank you for the precious information!

Before diving into this details I'm trying to make the MVP work by just applying the reference torque set from yarpmotorgui to the joint of the single pendulum, as done in gazebo-yarp-plugins here.

I'm having troubles doing this with gz-sim, since apparently using SetForce() is not working as expected.

In the PreUpdate() method of the plugin I'm getting the reference torque value, put that into a vector<double> as requested by the API and then calling

double forceReference = 42.0;
std::vector<double> forceVec{forceReference};
gzJoint.SetForce(_ecm, forceVec);

but when in the PostUpdate method I read the joint transmitted wrench (see https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1Joint.html#a2062bcf60d2cb8a990e8d906e7d4bab5) I get all zero values. I'm missing something 😕

[traversaro]

but when in the PostUpdate method I read the joint transmitted wrench (see https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1Joint.html#a2062bcf60d2cb8a990e8d906e7d4bab5) I get all zero values. I'm missing something 😕

Perhaps it is some problem in reading the joint trasmitted wrench, or the applied torque is dominated by the friction? In both cases, it may be beneficial to actually read the force set as done in the test for the SetForce function: https://github.com/gazebosim/gz-sim/blob/ab458ba79f316e3d9f768c71146519733c9ca00e/test/integration/joint.cc#L283-L292

[xela-95]

but when in the PostUpdate method I read the joint transmitted wrench (see https://gazebosim.org/api/sim/8/classgz_1_1sim_1_1Joint.html#a2062bcf60d2cb8a990e8d906e7d4bab5) I get all zero values. I'm missing something 😕

Perhaps it is some problem in reading the joint trasmitted wrench, or the applied torque is dominated by the friction? In both cases, it may be beneficial to actually read the force set as done in the test for the SetForce function: https://github.com/gazebosim/gz-sim/blob/ab458ba79f316e3d9f768c71146519733c9ca00e/test/integration/joint.cc#L283-L292

I've printed the JointForceCmd to read the force command I've sent, by using the same code found in the tests:

// check that the force cmd exists
double forceCmd = _ecm.Component<components::JointForceCmd>(gzJoint.Entity())->Data().at(0);
std::cerr << "forceCmd = " << std::to_string(forceCmd) << std::endl;

and I get exactly the same force applied.

Also from the model.sdf the joint has no friction. Maybe there's something wrong in the sdf?