Create YarpGazeboControlBoard and GazeboModelYarpPluginLoader

[TheArmega]

Create YarpGazeboControlBoard

• File and folder names inspired from https://github.com/roboticslab-uc3m/openrave-yarp-plugins/tree/master/libraries/YarpPlugins/YarpOpenraveControlBoard
• Copy or inherit from https://github.com/roboticslab-uc3m/yarp-devices/tree/master/libraries/YarpPlugins/EmulatedControlBoard
• Addiotional inspiration https://github.com/robotology/gazebo-yarp-plugins

[jgvictores]

Copy or inherit from https://github.com/roboticslab-uc3m/yarp-devices/tree/master/libraries/YarpPlugins/EmulatedControlBoard

Spoken with @PeterBowman the best approach seems to be using EmulatedControlBoard as a plugin within the newly developed plugin (composition, possibly having a private DeviceDriver within the newly developed plugin).

• File names inpired from https://github.com/roboticslab-uc3m/openrave-yarp-plugins/tree/master/libraries/YarpPlugins/YarpOpenraveControlBoard

Yes, as required taking into account the above!

[jgvictores]

PS: The idea is to avoid PID controllers, etc.

• Velocity commands can be forwarded directly to Gazebo similar to leftJoint->SetVelocity(..., ...); from my_wheels_g9.zip at TEÓfilos
• PositionDirect can also be forwarded directly to Gazebo as in leftJoint->SetPosition(..., ...);
• Position commands are the ones that would actually rely on EmulatedControlBoard for trajectory generation, thus:
  • The plugin's state should be some way updated/syncronized
  • Someting like a leftJoint->SetPosition(..., ...); should be called at each simulation timestep throughout all the duration of the generated trajectory.

[TheArmega]

I'm going to learn how to develop a Gazebo plugin in YARP from scratch, without reusing what the Robotology team has already done. For that, I'm going to look at https://classic.gazebosim.org/tutorials?tut=plugins_hello_world&cat=write_plugin

Gazebo plugins are nothing more than C++ classes that extend the functionality of the simulator, while YARP plugins (also called device drivers) are classes used in YARP to abstract functionality of devices used in robots.

Developing our plugin

• Install the Gazebo development files:

  
  sudo apt-get install libgazebo-dev

- Create a plugins directory and the file that will contain the plugin:
```bash 
mkdir ~/<gazebo_plugins_directory>
cd ~/<gazebo_plugins_directory>
gedit hello_world.cc

• Write the plugin:

  
  #include <gazebo/gazebo.hh>

namespace gazebo { class WorldPluginTutorial : public WorldPlugin { public: WorldPluginTutorial() : WorldPlugin() { printf("Hello World!\n"); }

public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf)
        {
        }

}; GZ_REGISTER_WORLD_PLUGIN(WorldPluginTutorial) }

## Compiling the plugin
- Create the CMakeLists.txt file:
```bash
gedit ~/<gazebo_plugins_directory>/CMakeLists.txt

cmake_minimum_required(VERSION 2.8 FATAL_ERROR)

find_package(gazebo REQUIRED)
include_directories(${GAZEBO_INCLUDE_DIRS})
link_directories(${GAZEBO_LIBRARY_DIRS})
list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS}")

add_library(hello_world SHARED hello_world.cc)
target_link_libraries(hello_world ${GAZEBO_LIBRARIES})

• Create the build directory and compile the plugin:

  
  mkdir ~/gazebo_plugin_tutorial/build
  cd ~/gazebo_plugin_tutorial/build
  cmake ..
  make

- The result of the compilation will be a .so file that can be inserted into the Gazebo simulation.
- Finally, add the plugin path to the Gazebo path:
``` bash
export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:~/<gazebo_plugins_directory>/build

Using the plugin

I managed to make the plugin work in two different ways:

cd ~/<gazebo_plugins_directory>/build
gazebo -s libhello_world.so ~/teo-gazebo-models/worlds/teo_fixed.world --verbose

<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">

<include>
  <uri>model://sun</uri>
</include>

<include>
  <uri>model://ground_plane</uri>
</include>

<include>
  <pose>0 0 0.05 0 -0 0</pose>
  <uri>model://teo_fixed</uri>
</include>

</world>
</sdf>

And also:

cd ~/<gazebo_plugins_directory>/build
gazebo ~/teo-gazebo-models/worlds/teo_fixed.world --verbose

<?xml version="1.0"?>
<sdf version="1.6">
<world name="default">

<include>
  <uri>model://sun</uri>
</include>

<include>
  <uri>model://ground_plane</uri>
</include>

<include>
  <pose>0 0 0.05 0 -0 0</pose>
  <uri>model://teo_fixed</uri>
</include>

<plugin name="hello_world" filename="libhello_world.so"/>

</world>
</sdf>

Direct position control plugin

This is the first step towards the final development of the direct position control plugin in Gazebo with YARP. This initial version of the plugin sends direct position commands every five seconds. It is just a test to verify that we can develop the plugin and that we are able to send position commands bypassing a PID controller.

#include <gazebo/gazebo.hh>
#include <yarp/os/all.h>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <unistd.h>

namespace gazebo
{
  class ArmMovement : public WorldPlugin
  {
    private: yarp::os::Network yarpNetwork;
    private: yarp::os::Port outPort;
    private: physics::ModelPtr robotModel;
    private: physics::JointControllerPtr jointController;
    private: event::ConnectionPtr updateConnection; // Added as private member
    double position = 0;

    public: ArmMovement() : WorldPlugin()
            {
              if (!yarpNetwork.checkNetwork())
              {
                printf("Error: Could not connect to YARP server.\\n");
                return;
              }

              outPort.open("/robotArmController/out");
              printf("Robot Arm Controller initialized! Connected to YARP.\\n");
            }

    public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf) override
            {
              this->robotModel = _world->ModelByName("TEO_fixed");

              if (!this->robotModel)
              {
                gzerr << "Robot model not found.\\n";
                return;
              }
              else
              {
                printf("Robot model found!\\n");
              }

              this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                std::bind(&ArmMovement::OnUpdate, this));
            }

    public: void OnUpdate()
            {
                position += 0.1;
                auto joints = this->robotModel->GetJoints();
                printf("Robot joints:\\n");
                for (const auto& joint : joints)
                {
                  printf(" - %s, Position: %d\\n", joint->GetName().c_str(), position);
                }

              auto joint = this->robotModel->GetJoint("TEO::TEO_rightArm::FrontalRightShoulder");
              if (joint)
              {
                printf("Right arm joint found!\\n");
                joint->SetVelocity(0, 0.5);
                joint->SetPosition(0, position);
                sleep(5);
              }
            }

    public: ~ArmMovement()
            {
              outPort.close();
            }
  };

  GZ_REGISTER_WORLD_PLUGIN(ArmMovement)
}

[jgvictores]

Hi @TheArmega !

Here's a gentle introduction to YARP devices (Spanish): https://apps-robots.uc3m.es/asrob/wiki/Tutorial_yarp_devices

The cool thing is you do not open ports explicitly! This makes the device/plugin mechanism very useful (e.g. switch between YARP ports or ROS topics, or even local use where applicable).

[jgvictores]

PS: Note that the yarpdev syntax has slightly changed, per https://github.com/robotology/yarp/discussions/3078

[TheArmega]

ASWJ @jgvictores We will replicate OpenraveYarpPluginLoader as https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/master/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp

[TheArmega]

ASWJ @jgvictores we will add the folder GazeboModelYarpPluginLoader

[jgvictores]

Btw @TheArmega I've been giving this some additional though and think we should not touch yarp-devices, and can stick with the original plan. We can comment :smiley:2:smiley: tomorrow!

[TheArmega]

We have implemented the following interfaces, inspired from EmulatedControlBoard in yarp-devices:

• IDeviceDriver
• IControlLimits
• IControlMode
• IEncoders
• IPositionControl
• IPositionDirect
• IVelocityControl
• PeriodicThread

All implementations are empty, but the plugin is compiling and detected by Yarp.