Open TheArmega opened 7 months ago
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).
Yes, as required taking into account the above!
PS: The idea is to avoid PID controllers, etc.
leftJoint->SetVelocity(..., ...);
from my_wheels_g9.zip
at TEÓfilos
leftJoint->SetPosition(..., ...);
EmulatedControlBoard
for trajectory generation, thus:
leftJoint->SetPosition(..., ...);
should be called at each simulation timestep throughout all the duration of the generated trajectory.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.
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
#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})
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
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>
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)
}
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).
PS: Note that the yarpdev
syntax has slightly changed, per https://github.com/robotology/yarp/discussions/3078
ASWJ @jgvictores We will replicate OpenraveYarpPluginLoader
as https://github.com/roboticslab-uc3m/openrave-yarp-plugins/blob/master/libraries/OpenravePlugins/OpenraveYarpPluginLoader/OpenraveYarpPluginLoader.cpp
ASWJ @jgvictores we will add the folder GazeboModelYarpPluginLoader
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!
We have implemented the following interfaces, inspired from EmulatedControlBoard
in yarp-devices
:
All implementations are empty, but the plugin is compiling and detected by Yarp.
Create YarpGazeboControlBoard